From 16bdb15aa0f05949b764c1655c853f06df039eb1 Mon Sep 17 00:00:00 2001
From: Robert Hedman <robert.hedman@mac.com>
Date: Tue, 28 Nov 2017 16:57:23 +0100
Subject: [PATCH] real lidar commands supported
---
src/main.rs | 69 +++++++++++++++++++++++++++++++++++++----------------
1 file changed, 49 insertions(+), 20 deletions(-)
diff --git a/src/main.rs b/src/main.rs
index e7dff4f..f5ed26d 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -25,9 +25,17 @@ const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
// should the map matrix be two dim array of const size, vecs of dynamic, or what?
const ROWS: usize = 50;
const COLS: usize = 100;
-static mut vec = vec![vec![0; COLS]; ROWS];
fn main() {
+
+ //let mut vecMap = vec![vec![0; ROWS]; COLS];
+
+
+ let mut map = [[0u32; ROWS]; COLS];
+ map[0][0] = 3;
+
+ println!("{}, {}", map[0][0], map[0][1]);
+
// dummy thread for replacing lidar
let server = thread::spawn(|| {
let listener = TcpListener::bind(LIDAR_ADDRESS).unwrap();
@@ -56,7 +64,8 @@ fn main() {
message_len+=1; // since length now is total bytes, not index
//println!("Server found end of message");
println!("Server got: {:?}", String::from_utf8_lossy(&message[0..message_len]));
- let written_size = stream.write(&[69 as u8, 96 as u8]);
+ //let written_size = stream.write(&[69 as u8, 96 as u8]);
+ let written_size = stream.write_all(b"\x02This is a bunch of lidar data...\x03");
println!("server wrote {:?} to stream.", written_size);
// reset everything for next message.
@@ -80,14 +89,10 @@ fn main() {
});
- let mut map = [[0u32; ROWS]; COLS];
- map[0][0] = 3;
-
- println!("{}, {}", map[0][0], map[0][1]);
-
let (request_location_tx, request_location_rx): (mpsc::Sender<bool>, mpsc::Receiver<bool>) = mpsc::channel();
let (send_location_tx, send_location_rx): (mpsc::Sender<(i32, i32)>, mpsc::Receiver<(i32, i32)>) = mpsc::channel();
+
// location tracker
let tracker = thread::spawn(move || {
println!("Tracker started.");
@@ -121,12 +126,12 @@ fn main() {
break;
};
send_location_tx.send(pos).unwrap();
- pos.0 += 1;
+ //pos.0 += 1;
},
Err(_) => {
//println!("Error: {}, on recieving side.", e);
//thread::sleep_ms(100);
- pos.1 += 1;
+ //pos.1 += 1;
},
};
};
@@ -144,7 +149,9 @@ fn main() {
// set up connection to lidar
let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap();
- let mut lidar_buf: [u8; 2] = [0; 2];
+ let mut lidar_buf: [u8; 4096] = [0u8; 4096];
+ let mut message_len = 0;
+ let mut recording = false;
// main loop for this thread
loop {
@@ -158,18 +165,40 @@ fn main() {
// request lidar data
//thread::sleep(time::Duration::new(3,0));
- let _ = stream.write_all(b"\x02sRN LMDscandata\x03");
- let stream_write_response = stream.write(&[65 as u8, 66 as u8]);
- println!("mapper wrote {:?} to stream.", stream_write_response);
+ let _ = stream.write_all(b"\x02sRN LMDscandata\x03"); // tell lidar to send one measurment
+ for r in stream.try_clone().expect("stream clone failed").bytes() { // iterate over answer
+ match r {
+ Ok(b) => {
+ if b == 2 && !recording { // start of message
+ //println!("Server found start of message");
+ lidar_buf[message_len] = b;
+ recording = true;
+ message_len+=1;
+ }
+ else if b == 3 { // end of message
+ lidar_buf[message_len] = b;
+ message_len+=1; // since length now is total bytes, not index
+ //println!("Server found end of message");
+ println!("Mapper got (from lidar): {:?}", String::from_utf8_lossy(&lidar_buf[0..message_len]));
+ //let written_size = stream.write(&[69 as u8, 96 as u8]);
+ //println!("server wrote {:?} to stream.", written_size);
+
+ // reset everything for next message.
+ recording = false;
+ message_len = 0;
+ break;
+ }
+ else if recording {
+ lidar_buf[message_len] = b;
+ message_len+=1;
+ }
- match stream.read_exact(&mut lidar_buf){
- Ok(_) => {
- println!("mapper got {}, {}, from lidar.", lidar_buf[0], lidar_buf[1]);
},
- Err(e) => {
- println!("Could not read all bytes: {:?}", e);
- },
- }
+ Err(_) => {},
+ }
+ }
+ //let stream_write_response = stream.write(&[65 as u8, 66 as u8]);
+ //println!("mapper wrote {:?} to stream.", stream_write_response);
// update map
--
GitLab