Initial rust rplidar implementation

.gitignore

@@ -0,0 +1,2 @@
+/target
+Cargo.lock

Cargo.toml

@@ -0,0 +1,9 @@
+[package]
+name = "rplidar-rs"
+version = "0.1.0"
+edition = "2021"
+
+[dependencies]
+
+# https://docs.rs/serialport/4.0.1/serialport/index.html
+serialport = "4.0.1"

src/lib.rs

@@ -0,0 +1,305 @@
+use serialport::ClearBuffer;
+use serialport::SerialPort;
+use std::io::Error;
+use std::string::FromUtf8Error;
+
+const SYNC: u8 = 0xA5;
+const SYNC2: u8 = 0x5A;
+
+const GET_INFO: u8 = 0x50;
+const GET_HEALTH: u8 = 0x52;
+
+const STOP: u8 = 0x25;
+const RESET: u8 = 0x40;
+
+const SCAN: u8 = 0x20;
+#[allow(dead_code)]
+const FORCE_SCAN: u8 = 0x21;
+
+const DESCRIPTOR_LEN: usize = 7;
+const INFO_LEN: u8 = 20;
+const HEALTH_LEN: u8 = 3;
+
+const INFO_TYPE: u8 = 4;
+const HEALTH_TYPE: u8 = 6;
+const SCAN_TYPE: u8 = 129;
+
+const SET_PWM: u8 = 0xF0;
+const MAX_MOTOR_PWM: usize = 1023;
+const DEFAULT_MOTOR_PWM: usize = 660;
+
+const SCAN_SIZE: u8 = 5;
+
+pub enum HealthStatus {
+    Good,
+    Warning,
+    Error,
+}
+
+impl HealthStatus {
+    pub fn from_raw(raw: u8) -> HealthStatus {
+        match raw {
+            0 => HealthStatus::Good,
+            1 => HealthStatus::Warning,
+            _ => HealthStatus::Error,
+        }
+    }
+}
+
+pub enum RPLidarError {
+    FailedToStart,
+    IncorrectInfoFormat,
+    IncorrectHealthFormat,
+    ScanError(String),
+    IncorrectDescriptorFormat,
+}
+
+impl From<Error> for RPLidarError {
+    fn from(_: Error) -> Self {
+        todo!()
+    }
+}
+
+impl From<FromUtf8Error> for RPLidarError {
+    fn from(_: FromUtf8Error) -> Self {
+        todo!()
+    }
+}
+
+pub struct LidarScan {
+    pub new_scan: bool,
+    pub quality: u8,
+    pub angle: f64,
+    pub distance: f64,
+}
+
+fn process_scan(raw: Vec<u8>) -> Result<LidarScan, RPLidarError> {
+    let new_scan = raw[0] & 0b1;
+    let inversed_new_scan = (raw[0] >> 1) & 0b1;
+    let quality = raw[0] >> 2;
+    if new_scan == inversed_new_scan {
+        return Err(RPLidarError::ScanError(String::from(
+            "New scan flags mismatch",
+        )));
+    }
+
+    if (raw[1] & 0b1) != 1 {
+        return Err(RPLidarError::ScanError(String::from(
+            "Check bit not equal to 1",
+        )));
+    }
+
+    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 {
+        new_scan: new_scan == 1,
+        quality,
+        angle,
+        distance,
+    })
+}
+
+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,
+    max_buffer_measurements: u32,
+}
+
+impl<T: SerialPort> Lidar<T> {
+    pub fn new(serial_port: T) -> Lidar<T> {
+        Lidar {
+            motor_running: false,
+            serial_port,
+            is_scanning: false,
+            measurements_per_batch: 200,
+            max_buffer_measurements: 500,
+        }
+    }
+
+    pub fn connect(&mut self) {
+        self.serial_port
+            .set_baud_rate(115200)
+            .expect("Failed to set baudrate");
+    }
+
+    pub fn start_motor(&mut self) {
+        self.serial_port
+            .write_data_terminal_ready(true)
+            .expect("Failed to write dtr");
+        self.set_pwm(DEFAULT_MOTOR_PWM);
+        self.motor_running = true;
+    }
+
+    pub fn start_scanning(&mut self) -> Result<u8, RPLidarError> {
+        self.start_motor();
+        let (status, _) = self.get_health()?;
+
+        match status {
+            HealthStatus::Error => Err(RPLidarError::IncorrectHealthFormat),
+            _ => {
+                self.send_command(vec![SCAN]);
+                let (data_size, is_single, data_type) = self.read_descriptor()?;
+                if data_size != SCAN_SIZE || is_single || data_type != SCAN_TYPE {
+                    return Err(RPLidarError::IncorrectDescriptorFormat);
+                }
+                self.is_scanning = true;
+                Ok(data_size)
+            }
+        }
+    }
+
+    pub fn stop_motor(&mut self) {
+        self.set_pwm(0);
+        self.serial_port
+            .write_data_terminal_ready(false)
+            .expect("Failed to write dtr");
+        self.motor_running = false;
+    }
+
+    pub fn get_info(&mut self) -> Result<(u8, (u8, u8), u8, String), RPLidarError> {
+        self.send_command(vec![GET_INFO]);
+
+        let (data_size, is_single, data_type) = self.read_descriptor()?;
+
+        if data_size != INFO_LEN || !is_single || data_type != INFO_TYPE {
+            return Err(RPLidarError::IncorrectInfoFormat);
+        }
+
+        let raw = self.read_response(data_size)?;
+        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> {
+        self.send_command(vec![GET_HEALTH]);
+
+        let (data_size, is_single, data_type) = self.read_descriptor()?;
+
+        if data_size != HEALTH_LEN || !is_single || data_type != HEALTH_TYPE {
+            return Err(RPLidarError::IncorrectHealthFormat);
+        }
+
+        let raw = self.read_response(data_size)?;
+        let status = HealthStatus::from_raw(raw[0]);
+        let error_code = raw[1] << 8 + raw[2];
+        return Ok((status, error_code));
+    }
+
+    pub fn clear_input(&mut self) {
+        self.serial_port
+            .clear(ClearBuffer::Input)
+            .expect("Failed to clear input buffer");
+    }
+
+    pub fn stop(&mut self) {
+        self.send_command(vec![STOP]);
+        self.is_scanning = false;
+    }
+
+    pub fn reset(&mut self) {
+        self.send_command(vec![RESET]);
+    }
+
+    pub fn receive_measurement_and_clear_buffer(
+        &mut self,
+        max_buffer_measurements: u32,
+    ) -> Result<Vec<u8>, RPLidarError> {
+        if !self.is_scanning {
+            return Err(RPLidarError::ScanError(String::from(
+                "Haven't started scanning",
+            )));
+        }
+        let raw = self.read_response(SCAN_SIZE)?;
+        if max_buffer_measurements > 0 {
+            if self.serial_port.bytes_to_read().unwrap()
+                > max_buffer_measurements * SCAN_SIZE as u32
+            {
+                println!("Too many measurements in the input buffer. Clearing Buffer");
+                self.serial_port
+                    .clear(ClearBuffer::Input)
+                    .expect("Failed to clear input buffer.");
+            }
+        }
+        Ok(raw)
+    }
+
+    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
+            .write(&vec)
+            .expect("Failed to send command");
+    }
+
+    fn send_payload_command(&mut self, cmd: u8, payload: Vec<u8>) {
+        let size: u8 = payload.len().try_into().unwrap();
+        let mut req = vec![SYNC];
+        req.push(cmd);
+        req.push(size);
+        req.extend(payload);
+        let checksum = self.calc_checksum(&req);
+        req.push(checksum);
+        self.serial_port
+            .write(&req)
+            .expect("Failed to send payload");
+    }
+
+    fn calc_checksum(&self, data: &Vec<u8>) -> u8 {
+        data.iter()
+            .copied()
+            .reduce(|accum, next| accum ^ next)
+            .unwrap()
+            .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) {
+            Err(RPLidarError::IncorrectDescriptorFormat)
+        } else {
+            let is_single = descriptor[DESCRIPTOR_LEN - 2] == 0;
+            Ok((descriptor[2], is_single, descriptor[DESCRIPTOR_LEN - 1]))
+        }
+    }
+
+    fn set_pwm(&mut self, pwm: usize) {
+        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
+impl<T: SerialPort> Iterator for Lidar<T> {
+    type Item = Vec<LidarScan>;
+
+    fn next(&mut self) -> Option<Self::Item> {
+        if self.is_scanning {
+            let mut all_scans: Vec<LidarScan> = Vec::new();
+            while all_scans.len() < self.measurements_per_batch {
+                if let Ok(raw) =
+                    self.receive_measurement_and_clear_buffer(self.max_buffer_measurements)
+                {
+                    if let Ok(scan) = process_scan(raw) {
+                        all_scans.push(scan);
+                    }
+                }
+            }
+            return Some(all_scans);
+        }
+        None
+    }
+}