real lidar commands supported

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