Select Git revision
main.rs 8.68 KiB
use std::thread;
use std::sync::mpsc;
use std::time;
// stuff for tcp
use std::io::prelude::*;
use std::net::TcpStream;
use std::net::TcpListener;
// stuff for serial
extern crate serial;
use std::io;
use std::time::Duration;
//use std::io::prelude::*;
use serial::prelude::*;
//use std::str;
// constants
const LIDAR_ADDRESS: &str = "127.0.0.1:8080";
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;
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();
let mut stream;
match listener.accept() {
Ok((socket, addr)) => {
println!("server: new client: {:?}", addr); stream=socket;
thread::sleep(time::Duration::new(1,0));
let mut message: [u8; 2048] = [0; 2048];
let mut message_len = 0;
let mut recording = false;
for r in stream.try_clone().expect("stream clone failed").bytes() {
match r {
Ok(b) => {
if b == 2 && !recording {
//println!("Server found start of message");
message[message_len] = b;
recording = true;
message_len+=1;
}
else if b == 3 {
message[message_len] = b;
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_all(b"\x02This is a bunch of lidar data...\x03");
println!("server wrote {:?} to stream.", written_size);
// reset everything for next message.
recording = false;
message_len = 0;
//break;
}
else if recording {
message[message_len] = b;
message_len+=1;
}
},
Err(_) => {},
}
}
},
Err(e) => println!("couldn't get client: {:?}", e),
}
});
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.");
let mut serial_buf: [u8; 8] = [0; 8];
let mut port = serial::open(&COUNTER_SERIAL_PORT).unwrap();
interact(&mut port).unwrap();
let mut pos: (i32, i32) = (0,0);
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);
print!("L: {}, ", pos.0);
println!("R: {}", pos.1);
},
Err(e) => {
println!("Could not read all bytes: {:?}", e);
},
}
// check if position is requested
match request_location_rx.try_recv() {
Ok(msg) => {
if msg == false {
println!("tracker got stop signal.");
break;
};
send_location_tx.send(pos).unwrap();
//pos.0 += 1;
},
Err(_) => {
//println!("Error: {}, on recieving side.", e);
//thread::sleep_ms(100);
//pos.1 += 1;
},
};
};
println!("tracker exiting with local pos: {:?}", pos);
});
// mapper (gets lidar data, requests position and updates its map.)
let mapper = thread::spawn(move || {
println!("mapper started.");
// create empty map
let mut pos: (i32, i32); //= (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 recording = false;
// main loop for this thread
loop {
// request position data
request_location_tx.send(true).unwrap();
pos = send_location_rx.recv().unwrap();
if pos.0 % 1000 == 0 {
println!("mapper pos: {:?} ",pos);
};
// request lidar data
//thread::sleep(time::Duration::new(3,0));
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;
}
},
Err(_) => {},
}
}
//let stream_write_response = stream.write(&[65 as u8, 66 as u8]);
//println!("mapper wrote {:?} to stream.", stream_write_response);
// update map
if pos.0 == 2000 {
println!("mapper done.");
request_location_tx.send(false).unwrap();
break;
} thread::sleep(time::Duration::new(2,0*50*1000000)); // from ms to ns
};
});
println!("Main wating for join.");
mapper.join().unwrap();
tracker.join().unwrap();
println!("Main wating for server join.");
//drop(stream);
server.join().unwrap();
println!("end of main");
// pathfinder, takes a map and returns a list of nodes to aim for when driving
// This will be a function for mapper to call.
}
fn interact<T: SerialPort>(port: &mut T) -> io::Result<()> {
try!(port.reconfigure(&|settings| {
try!(settings.set_baud_rate(serial::Baud9600));
settings.set_char_size(serial::Bits8);
settings.set_parity(serial::ParityNone);
settings.set_stop_bits(serial::Stop1);
settings.set_flow_control(serial::FlowNone);
Ok(())
}));
try!(port.set_timeout(Duration::from_millis(10000)));
Ok(())
}
fn freq_to_distance(freq: i32) -> i32 {
freq/10
}