new feature: rplidar_drv::sort_scan (ascendScan in C++)

reform: renamed rplidar_drv::protocol::RplidarProtocol to RplidarHostProtocol
improve: eliminated dependency to libudev to avoid cross compile failure on linux

Author: tony-slamtec

README.md

@@ -16,6 +16,7 @@ Please see [Roadmap](https://github.com/cnwzhjs/rplidar.rs/blob/master/docs/Road
 
 ## Release Notes
 
+* [v0.5.0](https://github.com/cnwzhjs/rplidar.rs/blob/master/docs/ReleaseNote.v0.5.0.md)
 * [v0.4.0](https://github.com/cnwzhjs/rplidar.rs/blob/master/docs/ReleaseNote.v0.4.0.md)
 * [v0.3.0](https://github.com/cnwzhjs/rplidar.rs/blob/master/docs/ReleaseNote.v0.3.0.md)
 * [v0.2.0](https://github.com/cnwzhjs/rplidar.rs/blob/master/docs/ReleaseNote.v0.2.0.md)
@@ -33,6 +34,13 @@ RPLIDAR SDK supports Windows, macOS and Linux by using Visual Studio 2010 projec
 
 ## Quick Start
 
+This crate works with Cargo and is on crates.io. Add it to your `Cargo.toml` like so:
+
+```toml
+[dependencies]
+rplidar_drv = "0.5.0"
+```
+
 To use RPLIDAR Rust SDK is quite simple:
 
 ```rust

docs/ReleaseNote.v0.5.0.md

@@ -0,0 +1,5 @@
+# Release Note for Slamtec RPLIDAR Public SDK for Rust v0.5.0
+
+* new feature: rplidar_drv::sort_scan (ascendScan in C++)
+* reform: renamed rplidar_drv::protocol::RplidarProtocol to RplidarHostProtocol
+* improve: eliminated dependency to libudev to avoid cross compile failure on linux

docs/Roadmap.md

@@ -15,7 +15,7 @@
 | feature - stop                         | since 0.1.0  |
 | feature - grab_scan                    | since 0.2.0  |
 | feature - grab_scan_point              | since 0.1.0  |
-| feature - sort_scan                    | No           |
+| feature - sort_scan                    | since 0.5.0  |
 | protocol - measurement_nodes           | since 0.1.0  |
 | protocol - capsuled_nodes              | since 0.1.0  |
 | protocol - ultra_capsuled_nodes        | since 0.3.0  |

examples/ultra_simple/Cargo.toml

@@ -1,11 +1,11 @@
 [package]
 name = "ultra_simple"
-version = "0.4.0"
+version = "0.5.0"
 authors = ["Tony Huang <[email protected]>"]
 edition = "2018"
 
 [dependencies]
 rplidar_drv = { path = "../../rplidar_drv" }
 rpos_drv = { path = "../../rpos_drv" }
-serialport = "3.1.0"
+serialport = { version = "3.1.0", default-features = false, features = [] }
 hex-slice = "0.1.4"

examples/ultra_simple/src/main.rs

@@ -5,7 +5,7 @@ extern crate serialport;
 
 use hex_slice::AsHex;
 
-use rplidar_drv::{Health, RplidarDevice, RplidarProtocol};
+use rplidar_drv::{Health, RplidarDevice, RplidarHostProtocol};
 use rpos_drv::{Channel, ErrorKind};
 use serialport::prelude::*;
 use std::time::Duration;
@@ -42,8 +42,8 @@ fn main() {
         .write_data_terminal_ready(false)
         .expect("failed to clear DTR");
 
-    let channel = Channel::<RplidarProtocol, serialport::SerialPort>::new(
-        RplidarProtocol::new(),
+    let channel = Channel::<RplidarHostProtocol, serialport::SerialPort>::new(
+        RplidarHostProtocol::new(),
         serial_port,
     );
 

rplidar_drv/Cargo.toml

@@ -1,7 +1,7 @@
 [package]
 name = "rplidar_drv"
 description = "Driver for Slamtec RPLIDAR series laser sensors"
-version = "0.4.0"
+version = "0.5.0"
 license = "GPLv3"
 repository = "https://github.com/cnwzhjs/rplidar.rs/rplidar_drv"
 keywords = ["Slamtec", "Rplidar", "Driver"]

rplidar_drv/src/lib.rs

@@ -14,6 +14,7 @@ mod checksum;
 mod cmds;
 mod prelude;
 mod protocol;
+pub mod utils;
 
 pub use self::prelude::*;
 
@@ -25,7 +26,7 @@ use self::capsuled_parser::parse_capsuled;
 use self::ultra_capsuled_parser::parse_ultra_capsuled;
 use self::checksum::Checksum;
 use self::cmds::*;
-pub use self::protocol::RplidarProtocol;
+pub use self::protocol::RplidarHostProtocol;
 use byteorder::{ByteOrder, LittleEndian};
 use rpos_drv::{Channel, Error, ErrorKind, Message, Result};
 use std::collections::VecDeque;
@@ -39,7 +40,7 @@ const RPLIDAR_GET_LIDAR_CONF_START_VERSION:u16 = ((1 << 8) | (24)) as u16;
 /// Rplidar device driver
 #[derive(Debug)]
 pub struct RplidarDevice<T: ?Sized> {
-    channel: Channel<RplidarProtocol, T>,
+    channel: Channel<RplidarHostProtocol, T>,
     cached_measurement_nodes: VecDeque<ScanPoint>,
     cached_prev_capsule: CachedPrevCapsule,
 }
@@ -93,10 +94,10 @@ where
     /// # Example
     /// ```compile_fail
     /// let mut serial_port = serialport::open(serial_port_name)?;
-    /// let channel = Channel::new(RplidarProtocol::new(), serial_port);
+    /// let channel = Channel::new(RplidarHostProtocol::new(), serial_port);
     /// let rplidar_device = RplidarDevice::new(channel);
     /// ```
-    pub fn new(channel: Channel<RplidarProtocol, T>) -> RplidarDevice<T> {
+    pub fn new(channel: Channel<RplidarHostProtocol, T>) -> RplidarDevice<T> {
         RplidarDevice {
             channel: channel,
             cached_measurement_nodes: VecDeque::with_capacity(RPLIDAR_DEFAULT_CACHE_DEPTH),
@@ -112,7 +113,7 @@ where
     /// let rplidar_device = RplidarDevice::with_stream(serial_port);
     /// ```
     pub fn with_stream(stream: Box<T>) -> RplidarDevice<T> {
-        RplidarDevice::<T>::new(rpos_drv::Channel::new(RplidarProtocol::new(), stream))
+        RplidarDevice::<T>::new(rpos_drv::Channel::new(RplidarHostProtocol::new(), stream))
     }
 
     /// get device info of the RPLIDAR
@@ -708,8 +709,3 @@ fn check_sync_and_checksum_hq(msg: &Message) -> Result<()> {
         return Ok(());
     }
 }
-
-#[cfg(test)]
-mod tests {
-
-}

rplidar_drv/src/prelude.rs

@@ -1,30 +1,33 @@
 use std::f32::consts::PI;
 use super::answers::RPLIDAR_RESP_HQ_FLAG_SYNCBIT;
+use std::cmp::Ordering;
 
 /// Scan point in a particular laser scan
-#[derive(Debug, Clone, PartialEq)]
+#[derive(Debug, Clone, Eq)]
 pub struct ScanPoint {
     pub angle_z_q14: u16,
     pub dist_mm_q2: u32,
     pub quality: u8,
     pub flag: u8,
 }
 
-/// Laser scan
-#[derive(Debug, Clone, PartialEq)]
-pub struct LaserScan {
-    pub points: Vec<ScanPoint>,
-}
-
 impl ScanPoint {
     pub fn angle(&self) -> f32 {
         return (self.angle_z_q14 as f32) / 16384f32 / 2f32 * PI;
     }
 
+    pub fn set_angle(&mut self, angle:f32) {
+        self.angle_z_q14 = (angle * 16384f32 * 2f32 / PI) as u16;
+    }
+
     pub fn distance(&self) -> f32 {
         return (self.dist_mm_q2 as f32) / 4000f32;
     }
 
+    pub fn set_distance(&mut self, dist: f32) {
+        self.dist_mm_q2 = (dist * 4000f32) as u32;
+    }
+
     pub fn is_sync(&self) -> bool {
         return (self.flag & RPLIDAR_RESP_HQ_FLAG_SYNCBIT) == RPLIDAR_RESP_HQ_FLAG_SYNCBIT;
     }
@@ -34,6 +37,27 @@ impl ScanPoint {
     }
 }
 
+impl Ord for ScanPoint {
+    fn cmp(&self, other: &ScanPoint) -> Ordering {
+        self.angle_z_q14.cmp(&other.angle_z_q14)
+    }
+}
+
+impl PartialOrd for ScanPoint {
+    fn partial_cmp(&self, other: &ScanPoint) -> Option<Ordering> {
+        Some(self.cmp(other))
+    }
+}
+
+impl PartialEq for ScanPoint {
+    fn eq(&self, other: &ScanPoint) -> bool {
+        self.angle_z_q14 == other.angle_z_q14
+        && self.dist_mm_q2 == other.dist_mm_q2
+        && self.quality == other.quality
+        && self.flag == other.flag
+    }
+}
+
 pub use std::result::Result;
 
 /// Description of a specific scan mode

rplidar_drv/src/protocol.rs

@@ -24,19 +24,19 @@ enum DecodeStatus {
     ReceiveResponse,
 }
 
-/// The implementation of RPLIDAR protocol
+/// The implementation of RPLIDAR host protocol
 #[derive(Debug, Clone, PartialEq)]
-pub struct RplidarProtocol {
+pub struct RplidarHostProtocol {
     status: DecodeStatus,
     ans_header: Vec<u8>,
     ans_flag: u8,
     response_size: usize,
     decoding_msg: Message, // decode_buffer: RingByteBuffer
 }
 
-impl RplidarProtocol {
-    pub fn new() -> RplidarProtocol {
-        RplidarProtocol {
+impl RplidarHostProtocol {
+    pub fn new() -> RplidarHostProtocol {
+        RplidarHostProtocol {
             status: DecodeStatus::WaitSyncByte(0),
             ans_header: Vec::new(),
             ans_flag: 0,
@@ -141,7 +141,7 @@ impl RplidarProtocol {
     }
 }
 
-impl ProtocolDecoder for RplidarProtocol {
+impl ProtocolDecoder for RplidarHostProtocol {
     /// Decode bytes and return consumed bytes and message
     fn decode(&mut self, buf: &[u8]) -> Result<(usize, Option<Message>)> {
         let mut i = 0;
@@ -177,7 +177,7 @@ impl ProtocolDecoder for RplidarProtocol {
     }
 }
 
-impl ProtocolEncoder for RplidarProtocol {
+impl ProtocolEncoder for RplidarHostProtocol {
     /// Encode message into byte array
     /// Always encode commands
     fn encode(&mut self, msg: &Message, bytes: &mut [u8]) -> Result<usize> {
@@ -260,7 +260,7 @@ mod tests {
 
     #[test]
     fn protocol_encode() {
-        let mut protocol = super::RplidarProtocol::new();
+        let mut protocol = super::RplidarHostProtocol::new();
 
         assert_eq!(
             encode(&mut protocol, &Message::new(0x25))

rplidar_drv/src/utils.rs

@@ -0,0 +1,89 @@
+use super::prelude::{ ScanPoint };
+use rpos_drv::{Error, ErrorKind, Result};
+use std::f32::consts::PI;
+
+const PI2:f32 = PI * 2f32;
+
+fn find_first_valid_index(scan: &Vec<ScanPoint>) -> Option<usize> {
+    for i in 0..scan.len() {
+        if scan[i].is_valid() {
+            return Some(i);
+        }
+    }
+
+    return None;
+}
+
+fn find_last_valid_index(scan: &Vec<ScanPoint>) -> Option<usize> {
+    for i in 0..scan.len() {
+        let id = scan.len() - i - 1;
+
+        if scan[i].is_valid() {
+            return Some(id);
+        }
+    }
+
+    return None;
+}
+
+fn tune_head(scan: &mut Vec<ScanPoint>, inc_origin_angle: f32) -> Result<()> {
+    if let Some(head_index) = find_first_valid_index(scan) {
+        let mut i = head_index;
+
+        while i != 0 {
+            i -= 1;
+            let mut expect_angle = scan[i + 1].angle() - inc_origin_angle;
+            if expect_angle < 0f32 {
+                expect_angle = 0f32;
+            }
+            scan[i].set_angle(expect_angle);
+        }
+
+        return Ok(());
+    } else {
+        return Err(Error::new(ErrorKind::OperationFail, "no valid point"));
+    }
+}
+
+fn tune_tail(scan: &mut Vec<ScanPoint>, inc_origin_angle: f32) -> Result<()> {
+    if let Some(tail_index) = find_last_valid_index(scan) {
+        for i in tail_index+1..scan.len() {
+            let mut expect_angle = scan[i - 1].angle() + inc_origin_angle;
+            if expect_angle > PI2 {
+                expect_angle -= PI2;
+            }
+            scan[i].set_angle(expect_angle);
+        }
+
+        return Ok(());
+    } else {
+        return Err(Error::new(ErrorKind::OperationFail, "no valid point"));
+    }
+}
+
+/// sort scan points
+pub fn sort_scan(scan: &mut Vec<ScanPoint>) -> Result<()> {
+    if scan.len() == 0 {
+        return Ok(());
+    }
+
+    let inc_origin_angle = PI2 / (scan.len() as f32);
+    
+    tune_head(scan, inc_origin_angle)?;
+    tune_tail(scan, inc_origin_angle)?;
+
+    let front_angle = scan[0].angle();
+    for i in 1..scan.len() {
+        if !scan[i].is_valid() {
+            let mut expect_angle = front_angle + (i as f32) * inc_origin_angle;
+            if expect_angle > PI2 {
+                expect_angle -= PI2;
+            }
+            scan[i].set_angle(expect_angle);
+        }
+    }
+
+    scan.sort();
+    
+    return Ok(());
+}

rpos_drv/src/lib.rs

@@ -11,11 +11,3 @@ mod ring_byte_buffer;
 pub use self::prelude::*;
 pub use self::channel::*;
 pub use self::ring_byte_buffer::RingByteBuffer;
-
-#[cfg(test)]
-mod tests {
-    #[test]
-    fn it_works() {
-        assert_eq!(2 + 2, 4);
-    }
-}