vato007/rplidar-rs
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Get the example working with a real lidar

Browse Source
This commit is contained in:
Piv
2022-03-30 18:35:19 +10:30
parent 1461814c27
commit bd300269dd
2 changed files with 58 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
examples/simple_example.rs Normal file
View File

@@ -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());
}
}

70
src/lib.rs
View File

@@ -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 {
todo!()
fn from(err: Error) -> Self {
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 raw = self.read_response(data_size)?;
let mut buf = [0; INFO_LEN as usize];
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