From 13ae060847be2b3a2ed3e98ba8cf7a4d280407c5 Mon Sep 17 00:00:00 2001
From: Robert Hedman <robert.hedman@mac.com>
Date: Tue, 28 Nov 2017 17:59:01 +0100
Subject: [PATCH] sync
---
src/main.rs | 46 ++++++++++++++++++++++++++++++++--------------
1 file changed, 32 insertions(+), 14 deletions(-)
diff --git a/src/main.rs b/src/main.rs
index f5ed26d..24ab9da 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -21,20 +21,24 @@ use serial::prelude::*;
const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
const COUNTER_SERIAL_PORT: &str = "/dev/cu.usbmodem1422";
+const LIDAR_BUF_SIZE: usize = 4096;
-// should the map matrix be two dim array of const size, vecs of dynamic, or what?
-const ROWS: usize = 50;
-const COLS: usize = 100;
+// each pixel represents 1dm*1dm, so 100*100mm.
+const ROWS: usize = 1000;
+const COLS: usize = 1000;
+#![feature(box_syntax)]
fn main() {
+ let mut map= box ([[0u32; ROWS]; COLS]);
+
//let mut vecMap = vec![vec![0; ROWS]; COLS];
- let mut map = [[0u32; ROWS]; COLS];
- map[0][0] = 3;
+ //let mut map = [[0u32; ROWS]; COLS];
+ //map[0][0] = 3;
- println!("{}, {}", map[0][0], map[0][1]);
+ //println!("{}, {}", map[0][0], map[0][1]);
// dummy thread for replacing lidar
let server = thread::spawn(|| {
@@ -66,7 +70,7 @@ fn main() {
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_all(b"\x02This is a bunch of lidar data...\x03");
- println!("server wrote {:?} to stream.", written_size);
+ println!("server wrote to stream.");
// reset everything for next message.
recording = false;
@@ -90,7 +94,7 @@ fn main() {
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();
+ let (send_location_tx, send_location_rx): (mpsc::Sender<(i32, i32, f32)>, mpsc::Receiver<(i32, i32, f32)>) = mpsc::channel();
// location tracker
@@ -102,13 +106,15 @@ fn main() {
interact(&mut port).unwrap();
- let mut pos: (i32, i32) = (0,0);
+ let mut pos: (i32, i32, f32) = (COLS as i32/2,ROWS as i32/2, 0.0);
+ println!("location initial position: {:?}", pos);
loop {
// get encoder data from arduino and update local variables
match port.read_exact(&mut serial_buf){
Ok(_) => {
pos.0 += freq_to_distance( ( ((serial_buf[0] as u32) << 24) + ((serial_buf[1] as u32) << 16) + ((serial_buf[2] as u32) << 8) + (serial_buf[3] as u32) ) as i32);
pos.1 += freq_to_distance( ( ((serial_buf[4] as u32) << 24) + ((serial_buf[5] as u32) << 16) + ((serial_buf[6] as u32) << 8) + (serial_buf[7] as u32) ) as i32);
+ pos.2 = dist_to_angle(&pos);
print!("L: {}, ", pos.0);
println!("R: {}", pos.1);
},
@@ -125,7 +131,7 @@ fn main() {
println!("tracker got stop signal.");
break;
};
- send_location_tx.send(pos).unwrap();
+ send_location_tx.send((pos)).unwrap();
//pos.0 += 1;
},
Err(_) => {
@@ -145,17 +151,19 @@ fn main() {
let mapper = thread::spawn(move || {
println!("mapper started.");
// create empty map
- let mut pos: (i32, i32); //= (0,0);
+
+ let mut pos: (i32, i32, f32); //= (0,0);
// set up connection to lidar
let mut stream = TcpStream::connect(&LIDAR_ADDRESS).unwrap();
- let mut lidar_buf: [u8; 4096] = [0u8; 4096];
- let mut message_len = 0;
+ let mut lidar_buf: [u8; LIDAR_BUF_SIZE] = [0u8; LIDAR_BUF_SIZE];
+ let mut message_len: usize = 0;
let mut recording = false;
// main loop for this thread
loop {
// request position data
+ println!("Mapper requesting pos data.");
request_location_tx.send(true).unwrap();
pos = send_location_rx.recv().unwrap();
if pos.0 % 1000 == 0 {
@@ -165,6 +173,7 @@ fn main() {
// request lidar data
//thread::sleep(time::Duration::new(3,0));
+ println!("mapper requesting lidar scan.");
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 {
@@ -183,9 +192,12 @@ fn main() {
//let written_size = stream.write(&[69 as u8, 96 as u8]);
//println!("server wrote {:?} to stream.", written_size);
+ // update map using lidar data
+ update_map_with_lidar(lidar_buf, message_len, &mut map);
+
// reset everything for next message.
recording = false;
- message_len = 0;
+ let message_len: usize = 0;
break;
}
else if recording {
@@ -250,4 +262,10 @@ fn interact<T: SerialPort>(port: &mut T) -> io::Result<()> {
fn freq_to_distance(freq: i32) -> i32 {
freq/10
+}
+fn dist_to_angle(pos: &(i32,i32,f32)) -> f32 {
+ 0.0
+}
+fn update_map_with_lidar(lidar_data: [u8; LIDAR_BUF_SIZE], message_len: usize, map: &mut [[u32; ROWS]; COLS]) {
+ map[0][0] += 1;
}
\ No newline at end of file
--
GitLab