Get the example working with a real lidar

examples/simple_example.rs

@@ -0,0 +1,14 @@
+use rplidar_rs::Lidar;
+use std::time::Duration;
+
+fn main() {
+    let serial = serialport::new("/dev/cu.usbserial-0001", 115200)
+        .timeout(Duration::from_secs(10))
+        .open_native()
+        .expect("Failed to open serial port, check it's available");
+    let mut lidar = Lidar::new(serial);
+    lidar.start_scanning().expect("Failed to start scanning");
+    for scan in lidar {
+        println!("{}", scan.len());
+    }
+}

src/lib.rs

@@ -1,3 +1,4 @@
+use core::fmt;
 use serialport::ClearBuffer;
 use serialport::SerialPort;
 use std::io::Error;
@@ -26,6 +27,7 @@ const SCAN_TYPE: u8 = 129;
 
 const SET_PWM: u8 = 0xF0;
 const MAX_MOTOR_PWM: usize = 1023;
+#[allow(dead_code)]
 const DEFAULT_MOTOR_PWM: usize = 660;
 
 const SCAN_SIZE: u8 = 5;
@@ -52,17 +54,31 @@ pub enum RPLidarError {
     IncorrectHealthFormat,
     ScanError(String),
     IncorrectDescriptorFormat,
+    IOError(Error),
 }
 
 impl From<Error> for RPLidarError {
-    fn from(_: Error) -> Self {
+    fn from(err: Error) -> Self {
-        todo!()
+        RPLidarError::IOError(err)
     }
 }
 
 impl From<FromUtf8Error> for RPLidarError {
     fn from(_: FromUtf8Error) -> Self {
-        todo!()
+        RPLidarError::IncorrectDescriptorFormat
+    }
+}
+
+impl fmt::Debug for RPLidarError {
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        match self {
+            Self::FailedToStart => write!(f, "FailedToStart"),
+            Self::IncorrectInfoFormat => write!(f, "IncorrectInfoFormat"),
+            Self::IncorrectHealthFormat => write!(f, "IncorrectHealthFormat"),
+            Self::ScanError(arg0) => f.debug_tuple("ScanError").field(arg0).finish(),
+            Self::IncorrectDescriptorFormat => write!(f, "IncorrectDescriptorFormat"),
+            Self::IOError(arg0) => f.debug_tuple("IOError").field(arg0).finish(),
+        }
     }
 }
 
@@ -73,7 +89,7 @@ pub struct LidarScan {
     pub distance: f64,
 }
 
-fn process_scan(raw: Vec<u8>) -> Result<LidarScan, RPLidarError> {
+fn process_scan(raw: [u8; SCAN_SIZE as usize]) -> Result<LidarScan, RPLidarError> {
     let new_scan = raw[0] & 0b1;
     let inversed_new_scan = (raw[0] >> 1) & 0b1;
     let quality = raw[0] >> 2;
@@ -89,7 +105,7 @@ fn process_scan(raw: Vec<u8>) -> Result<LidarScan, RPLidarError> {
         )));
     }
 
-    let angle = (((raw[1] as isize) >> 1 + (raw[2] as isize) << 7) as f64) / 64.;
+    let angle = ((((raw[1] as isize) >> 1) + (raw[2] as isize) << 7) as f64) / 64.;
     let distance = (((raw[3] as isize) + ((raw[4] as isize) << 8)) as f64) / 4.;
 
     Ok(LidarScan {
@@ -102,8 +118,6 @@ fn process_scan(raw: Vec<u8>) -> Result<LidarScan, RPLidarError> {
 
 pub struct Lidar<T: SerialPort> {
     motor_running: bool,
-    // TODO: There's a good chance we'll only be able to get back a dyn serialport, which
-    // sucks as then this will need to be dyn as well.
     serial_port: T,
     is_scanning: bool,
     measurements_per_batch: usize,
@@ -131,7 +145,7 @@ impl<T: SerialPort> Lidar<T> {
         self.serial_port
             .write_data_terminal_ready(true)
             .expect("Failed to write dtr");
-        self.set_pwm(DEFAULT_MOTOR_PWM);
+        // self.set_pwm(DEFAULT_MOTOR_PWM);
         self.motor_running = true;
     }
 
@@ -169,13 +183,14 @@ impl<T: SerialPort> Lidar<T> {
         if data_size != INFO_LEN || !is_single || data_type != INFO_TYPE {
             return Err(RPLidarError::IncorrectInfoFormat);
         }
-
+        let mut buf = [0; INFO_LEN as usize];
-        let raw = self.read_response(data_size)?;
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
         let serial_number = String::from_utf8(Vec::from(&raw[4..]))?;
         return Ok((raw[0], (raw[2], raw[1]), raw[3], serial_number));
     }
 
-    pub fn get_health(&mut self) -> Result<(HealthStatus, u8), RPLidarError> {
+    pub fn get_health(&mut self) -> Result<(HealthStatus, usize), RPLidarError> {
         self.send_command(vec![GET_HEALTH]);
 
         let (data_size, is_single, data_type) = self.read_descriptor()?;
@@ -184,9 +199,11 @@ impl<T: SerialPort> Lidar<T> {
             return Err(RPLidarError::IncorrectHealthFormat);
         }
 
-        let raw = self.read_response(data_size)?;
+        let mut buf = [0; HEALTH_LEN as usize];
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
         let status = HealthStatus::from_raw(raw[0]);
-        let error_code = raw[1] << 8 + raw[2];
+        let error_code = (raw[1] as usize) << 8 + raw[2];
         return Ok((status, error_code));
     }
 
@@ -208,15 +225,20 @@ impl<T: SerialPort> Lidar<T> {
     pub fn receive_measurement_and_clear_buffer(
         &mut self,
         max_buffer_measurements: u32,
-    ) -> Result<Vec<u8>, RPLidarError> {
+    ) -> Result<[u8; SCAN_SIZE as usize], RPLidarError> {
         if !self.is_scanning {
             return Err(RPLidarError::ScanError(String::from(
                 "Haven't started scanning",
             )));
         }
-        let raw = self.read_response(SCAN_SIZE)?;
+        let mut buf = [0; SCAN_SIZE as usize];
+        self.serial_port.read(&mut buf)?;
+        let raw = buf;
         if max_buffer_measurements > 0 {
-            if self.serial_port.bytes_to_read().unwrap()
+            if self
+                .serial_port
+                .bytes_to_read()
+                .expect("Failed to get bytes to read")
                 > max_buffer_measurements * SCAN_SIZE as u32
             {
                 println!("Too many measurements in the input buffer. Clearing Buffer");
@@ -229,7 +251,6 @@ impl<T: SerialPort> Lidar<T> {
     }
 
     fn send_command(&mut self, command: Vec<u8>) {
-        // TODO: More performant way to do this? It doesn't really matter as it's just sending a command
         let mut vec = vec![SYNC];
         vec.extend(command);
         self.serial_port
@@ -238,7 +259,10 @@ impl<T: SerialPort> Lidar<T> {
     }
 
     fn send_payload_command(&mut self, cmd: u8, payload: Vec<u8>) {
-        let size: u8 = payload.len().try_into().unwrap();
+        let size: u8 = payload
+            .len()
+            .try_into()
+            .expect("Failed to convert payload length");
         let mut req = vec![SYNC];
         req.push(cmd);
         req.push(size);
@@ -254,15 +278,15 @@ impl<T: SerialPort> Lidar<T> {
         data.iter()
             .copied()
             .reduce(|accum, next| accum ^ next)
-            .unwrap()
+            .expect("Failed to calculate checksum")
             .to_owned()
     }
 
-    // TODO: Custom error type and unwrap the Option so we just return a result
     fn read_descriptor(&mut self) -> Result<(u8, bool, u8), RPLidarError> {
         let mut descriptor: [u8; DESCRIPTOR_LEN] = [0; DESCRIPTOR_LEN];
         let ret = self.serial_port.read(&mut descriptor)?;
         if ret != DESCRIPTOR_LEN || (descriptor[0] != SYNC && descriptor[1] != SYNC2) {
+            eprintln!("Failed to read enough or something");
             Err(RPLidarError::IncorrectDescriptorFormat)
         } else {
             let is_single = descriptor[DESCRIPTOR_LEN - 2] == 0;
@@ -274,12 +298,6 @@ impl<T: SerialPort> Lidar<T> {
         assert!(pwm <= MAX_MOTOR_PWM);
         self.send_payload_command(SET_PWM, Vec::from(pwm.to_ne_bytes()));
     }
-
-    fn read_response(&mut self, data_size: u8) -> Result<Vec<u8>, RPLidarError> {
-        let mut buf: Vec<u8> = Vec::with_capacity(data_size as usize);
-        self.serial_port.read(&mut buf)?;
-        Ok(buf)
-    }
 }
 
 // Probably want to have separate iterator types like we do in swift for measurements vs scans