complete rustdecisiondemo.

src/RUST/rustdecisiondemo/Readme.md

@@ -1,10 +1,36 @@
 在modulecomm目录编译modulecommrust生成libmodulecommrust.so文件拷贝到本目录。
 
-rust编译
-cargo rustc -- -L .   -lmodulecommrust
-
-
 protobuf:
 cargo install protobuf-codegen
 在.bashrc里增加
 export PATH=$PATH:/home/(user))/.cargo/bin
+
+进入src下的proto目录,执行
+protoc --rust_out=./ *.proto  生成proto的.rs文件。
+
+rust编译
+cargo build
+#cargo rustc -- -L .   -lmodulecommrust
+
+调试运行
+在home目录的.bashrc文件里增加
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:{libmodulecommrust.so所在目录}
+然后
+cargo build
+
+调试的launch.json内容如下：
+{  
+    "version": "0.2.0",  
+    "configurations": [  
+        {  
+            "name": "Debug executable 'rustdecisiondemo'",  
+            "type": "lldb",  
+            "request": "launch",  
+            "program": "${workspaceFolder}/target/debug/rustdecisiondemo",  
+            "args": [],  
+            "cwd": "${workspaceFolder}",  
+            "console": "integratedTerminal"  
+        }  
+    ]  
+}
+

src/RUST/rustdecisiondemo/src/gnssconvert.rs

@@ -35,6 +35,7 @@ pub fn gauss_proj_cal(longitude : f64, latitude:f64) -> (f64,f64){
 }
 
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+#[allow(unused_variables)]  
 pub fn gauss_proj_inv_cal(X: f64, Y: f64) -> (f64,f64){
 
     let i_pi: f64 = 0.0174532925199433; ////3.1415926535898/180.0;

src/RUST/rustdecisiondemo/src/main.rs

@@ -5,11 +5,12 @@ mod gnssconvert;
 use gnssconvert::gauss_proj_cal;
 
 
-use proto::object;
-use proto::objectarray;
+//use proto::object;
+//use proto::objectarray;
+use proto::decition;
 use proto::mapdata::Tracemap;
 use proto::mapdata::Mappoint;
-use proto::gpsimu::{self, Gpsimu};
+use proto::gpsimu::Gpsimu;
 use proto::objectarray::Objectarray;
 use protobuf::Message;
 extern  crate protobuf;
@@ -21,6 +22,10 @@ use std::time::Duration;
 use std::sync::Mutex;  
 use lazy_static::lazy_static;
 
+use std::time::{SystemTime, UNIX_EPOCH};  
+
+//use std::chrono::{Duration, Local};  
+
 
 struct Point2D{
     mx : f64,
@@ -39,8 +44,8 @@ struct  Adcdes{
 }
 
 impl Adcdes{
-    fn GetDecision(&mut self) -> f32{
-        println!("in decision xtr Size: {} update {}", self.xtr.point.len(),self.bxtrupdate);  
+    fn get_decision(&mut self, padecision: *mut std::ffi::c_void) -> f32{
+  //      println!("in decision xtr Size: {} update {}", self.xtr.point.len(),self.bxtrupdate);  
         if self.bxtrupdate == false {
             return 0.0;
         }
@@ -50,9 +55,48 @@ impl Adcdes{
         }
         
 
-        calcdecisoindemo(&self.xgpsimu,&self.xtr,&self.xobjarray,self.nobjupdate);
+        let (acc,wheel,speed) = calcdecisoindemo(&self.xgpsimu,&self.xtr,&self.xobjarray,self.nobjupdate);
+
+        let mut xdecision: decition::Decition = proto::decition::Decition::new();
         
+        xdecision.set_wheelAngle(wheel as f32);
+        xdecision.set_accelerator(acc as f32);
+        xdecision.set_brake(0.0_f32);
+        xdecision.set_speed(speed as f32);
+        if acc < 0.0{
+            xdecision.set_brake(acc as f32);
+            xdecision.set_torque(0.0 as f32);
+        }
+        else{
+            xdecision.set_brake(0.0_f32);
+            let f_veh_weight: f64 = 1800.0;
+ //#           fg = 9.8
+            let f_roll_force: f64 = 50.0;
+            let f_ratio: f64 = 2.5;
+            let f_need: f64 = f_roll_force + f_veh_weight*acc;
+            xdecision.set_torque((f_need/f_ratio) as f32);
+        }
+   
+        
+        match xdecision.write_to_bytes() {
+            Ok(v) => {
+                let len: u32 = v.len() as u32;
+                let u8_ptr = v.as_ptr() as *const c_char;
+ //               let c_str_ptr = vec_to_c_char(v);  
+                unsafe { modulecommrust::ModuleSendMsgRUST(padecision,u8_ptr,len) };
+            }
+
+            Err(e) =>{
+                println!("Error serialzie deciton protobuf: {:?}", e);  
+            }
+            
+        }
+
+     //   xdecision.write_to_bytes()  .unwrap();  
         
+        if self.nobjupdate >0 {
+            self.nobjupdate =self.nobjupdate -1;
+        }
         self.bgpsimuupdate = false;
         1.0
     }
@@ -63,7 +107,7 @@ lazy_static! {
     static ref DES:Mutex<Adcdes> = Mutex::new(Adcdes {xtr:Tracemap::new(),bxtrupdate : false,
                 xgpsimu:Gpsimu::new(),bgpsimuupdate:false,xobjarray:Objectarray::new(),nobjupdate:0});
 
-}  
+}   
 
 
 fn find_near_index(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
@@ -74,7 +118,7 @@ fn find_near_index(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
     let xpoints: &Vec<Mappoint> = &(xtracemap.point);
 
     for xp in xpoints.iter(){
-        println!("point,x: {} y: {}",xp.gps_x(),xp.gps_y());
+  //      println!("point,x: {} y: {}",xp.gps_x(),xp.gps_y());
         let dis = ((x - xp.gps_x())*(x - xp.gps_x()) + (y - xp.gps_y())*(y - xp.gps_y())).sqrt();
         let mut headingdiff = xgpsimu.heading() - xp.ins_heading_angle();
         while headingdiff < -180.0{
@@ -99,17 +143,17 @@ fn find_near_index(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
 }
 
 
-fn calcdecisoindemo(xgpsimu:&Gpsimu,xtracemap : &Tracemap,xobjarray : &Objectarray,nobjarrayupdate : i32) -> f32{
+fn calcdecisoindemo(xgpsimu:&Gpsimu,xtracemap : &Tracemap,xobjarray : &Objectarray,nobjarrayupdate : i32) -> (f64,f64,f64){
     let mendacc: f64 = -0.7;
     let nearindex: i32 = find_near_index(xgpsimu, xtracemap);
     if nearindex<0 {
         println!("not found near point");
-        return -1.0;
+        return (-1.0,0.0,0.0);
     }
     let (localpoints,distoend) = calclocalpath(nearindex,xgpsimu,xtracemap);
     
     let realspeed: f64 = 3.6 * ((xgpsimu.vn()).powf(2.0) + (xgpsimu.ve()).powf(2.0)).sqrt();
-    let  (mut acc,mut wheel,mut speed) = calccmd(localpoints, realspeed,xobjarray,nobjarrayupdate);
+    let  (mut acc,wheel,speed) = calccmd(localpoints, realspeed,xobjarray,nobjarrayupdate);
 
     let mut endacc: f64 = 0.0;
     if distoend > 0.1{
@@ -128,15 +172,17 @@ fn calcdecisoindemo(xgpsimu:&Gpsimu,xtracemap : &Tracemap,xobjarray : &Objectarr
     if acc < -5.0{
         acc = -5.0
     }
-    1.0
+    (acc,wheel,speed)
 }
 
 fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,nobjarrayupdate : i32) -> (f64,f64,f64){
     let desiredspeed: f64 = 10.0;
     let defaultacc: f64 = 1.0;
     let maxwheel: f64 = 430.0;
-    let gridsize = 0.2;
-    let mvehwidth = 2.3;
+    let gridsize: f64 = 0.2;
+    let mvehwidth: f64 = 2.3;
+    let mstopdistoobs: f64 = 6.0;
+    let mstopdisacc: f64 = -0.7;
     let mut pd: f64 = realspeed * 0.3;
     if pd < 4.0 { 
         pd = 4.0;
@@ -158,8 +204,8 @@ fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,
         }
         i = i+1;
     }
-    let mut acc = -0.5;
-    let mut wheel = 0.0;
+    let mut acc: f64 = -0.5;
+    let mut wheel: f64 ;
     if ppindex < 3{
         acc = 0.0;
         wheel = 0.0;
@@ -190,30 +236,30 @@ fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,
         }
     }
 
-    let mut obsdis = 0.0;
-    let mut bhaveobs = false;
+    let mut obsdis: f64 = 0.0;
+    let mut bhaveobs: bool = false;
 
     i = 0;
 
-    let mut gridcount = (((mvehwidth/2.0)/gridsize)*2.0) as i32;
-    let xwid = gridcount as f64 * gridsize;
+    let mut gridcount: i32 = (((mvehwidth/2.0)/gridsize)*2.0) as i32;
+    let xwid: f64 = gridcount as f64 * gridsize;
     if xwid < mvehwidth{
         gridcount = gridcount +2;
     }
 
     while i< size{
-        let ui = i as usize;
+        let ui: usize = i as usize;
         if i > 0{
             obsdis = obsdis + ((localpoints[ui].mx - localpoints[ui-1].mx).powf(2.0)
                                     +(localpoints[ui].my - localpoints[ui-1].my).powf(2.0)).sqrt();
         }
 
-        let nobjsize = xobjarray.obj.len();
-        let mut j = 0;
-        let mut bprob = false;//  #查看有没有可能
-        let mut k = 0;
+        let nobjsize: i32 = xobjarray.obj.len() as i32;
+        let mut j: i32 ;
+        let mut bprob: bool = false;//  #查看有没有可能
+        let mut k: i32 = 0;
         while k < nobjsize{
-            let uk = k as usize;
+            let uk: usize = k as usize;
             let dis = ((localpoints[ui].mx - xobjarray.obj[uk].position.x() as f64).powf(2.0)+ (localpoints[ui].my - xobjarray.obj[uk].position.y() as f64).powf(2.0)).sqrt();
             if (dis < (mvehwidth/2.0 + xobjarray.obj[uk].dimensions.x() as f64)) || (dis < (mvehwidth/2.0 + xobjarray.obj[uk].dimensions.y() as f64)){
                 bprob = true;
@@ -229,15 +275,25 @@ fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,
         }
         j =0;
         while j <= gridcount {
-            let off = (j as f64 - gridcount as f64/2.0 )*gridsize;
-            let xpos = localpoints[ui].mx + off * (localpoints[ui].mhdg + std::f64::consts::PI/2.0).cos();
-            let ypos = localpoints[ui].my + off * (localpoints[ui].mhdg + std::f64::consts::PI/2.0).sin();
- //           for pobj in xobjarray.obj:
- //           bres = self.is_point_in_rotated_rectangle(xpos,ypos,pobj.position.x,pobj.position.y,pobj.tyaw,
- //                                                     pobj.dimensions.x,pobj.dimensions.y)
- //           if bres == True:
- //               bhaveobs = True
- //               break
+            let off: f64 = (j as f64 - gridcount as f64/2.0 )*gridsize;
+            let xpos: f64 = localpoints[ui].mx + off * (localpoints[ui].mhdg + std::f64::consts::PI/2.0).cos();
+            let ypos: f64 = localpoints[ui].my + off * (localpoints[ui].mhdg + std::f64::consts::PI/2.0).sin();
+            k = 0;
+            while (k < nobjsize) && (nobjarrayupdate > 0){
+                let uk: usize = k as usize;
+                let x1: f64 = xobjarray.obj[uk].position.x() as f64;
+                let y1: f64 = xobjarray.obj[uk].position.y() as f64;
+                let yaw: f64 = xobjarray.obj[uk].tyaw() as f64;
+                let l: f64 = xobjarray.obj[uk].dimensions.x() as f64;
+                let w: f64 = xobjarray.obj[uk].dimensions.y() as f64;
+                let bres: bool = is_point_in_rotated_rectangle(xpos,ypos,x1,y1,yaw,
+                                                                         l,w);
+                 if bres == true {
+                    bhaveobs = true;
+                    break;
+                 }
+                k = k+1;
+            }
             if bhaveobs == true{
                 break
             }
@@ -249,19 +305,41 @@ fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,
         i = i+1;
     }
 
+    if bhaveobs == true {
+        println!("obs dis: {} ",obsdis);  
+        let vel: f64 = realspeed/3.6;
+        let srange: f64 = vel*vel/(2.0*(((mstopdisacc).abs())/2.0)) + mstopdistoobs;
+        if obsdis <= srange{
+            if obsdis < (mstopdistoobs + 0.1){
+                let stopacc: f64  = -3.0;
+                if acc > stopacc{
+                    acc = stopacc;
+                }
+            }
+            else {
+                let stopacc = -1.0*vel * vel/(2.0*(obsdis - mstopdistoobs));
+                if acc > stopacc{
+                    acc = stopacc;
+                }
+            }
+        }
+        if obsdis < mstopdistoobs * 1.1 {
+            if acc >= 0.0{
+                acc = mstopdisacc;
+            }
+        }
+    }
+
     let uppindex: usize = ppindex as usize;
     let denominator: f64 = 2.0 * localpoints[uppindex].mx *(-1.0);
     let numerator = (localpoints[uppindex].mx).powf(2.0) + (localpoints[uppindex].my).powf(2.0);
-    let mut fRadius: f64 = 1e9;
+    let  fRadius: f64 ;
     if (denominator).abs()>0.0{
         fRadius = numerator/denominator;
     }
     else{
         fRadius = 1e9;
     }
-    if fRadius == 0.0{
-        wheel = 0.0;
-    }
     let kappa: f64 = 1.0/fRadius;
     let wheel_base: f64 = 2.9;
     let wheelratio: f64 = 13.6;
@@ -273,10 +351,41 @@ fn calccmd(localpoints : Vec<Point2D>, realspeed : f64,xobjarray : &Objectarray,
         wheel = maxwheel * (-1.0);
     }
 
+    let now_utc = SystemTime::now();  
+    // 将SystemTime转换为自UNIX纪元（1970-01-01 00:00:00 UTC）以来的纳秒数  
+    let duration_since_epoch = now_utc.duration_since(UNIX_EPOCH).expect("Time went backwards!");  
+    println!("{} acc: {}  wheel: {}",duration_since_epoch.as_secs(), acc,wheel);
+
     (acc,wheel,desiredspeed)
 
 }
 
+fn is_point_in_rotated_rectangle(x:f64, y:f64, x1:f64, y1:f64, yaw:f64, l:f64, w:f64) -> bool{  
+    //# 将长方形的左下角坐标转换到原点  
+    let x_rel = x - x1;  
+    let y_rel = y - y1 ; 
+
+    //# 计算旋转矩阵（逆时针旋转）  
+    //# | cos(yaw)  -sin(yaw) |  
+    //# | sin(yaw)   cos(yaw) |  
+    let cos_yaw = (yaw).cos();  
+    let sin_yaw = (yaw).sin(); 
+
+    //# 应用旋转矩阵到相对坐标  
+    let x_rotated = x_rel * cos_yaw + y_rel * sin_yaw; 
+    let y_rotated = -x_rel * sin_yaw + y_rel * cos_yaw;  
+
+    //# 判断点是否在旋转后的长方形内  
+    //# 长方形的边界在旋转后的坐标系中是 [-l/2, l/2] x [-w/2, w/2]  
+    if (-l/2.0 <= x_rotated)&&(x_rotated <= l/2.0)&&(-w/2.0 <= y_rotated)&&(y_rotated <= w/2.0){  
+        return  true;
+    }
+    else{ 
+        return false;
+    }
+
+}
+
 fn calclocalpath(index:i32,xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> (Vec<Point2D>,f64){
     let mut localpoints = Vec::new();
     let mut distoend: f64 = 0.0;
@@ -317,7 +426,7 @@ fn calclocalpath(index:i32,xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> (Vec<Point2
         }
         i = i+1;
         if distoend > 300.0 {
-            break;;
+            break;
         }
     }
 
@@ -373,7 +482,7 @@ extern "C" fn tracemap_callback(strdata: *const c_char, nsize: c_uint, index: c_
        
 } 
 
-extern "C" fn gpsimu_callback(strdata: *const c_char, nsize: c_uint, index: c_uint) {  
+extern "C" fn gpsimu_callback(strdata: *const c_char, nsize: c_uint, _ : c_uint) {  
     let n_size: usize = nsize as usize;  
     let bytes = c_str_to_bytes(strdata, n_size); 
     match proto::gpsimu::Gpsimu::parse_from_bytes(&bytes) {  
@@ -390,7 +499,7 @@ extern "C" fn gpsimu_callback(strdata: *const c_char, nsize: c_uint, index: c_ui
     } 
 }
 
-extern "C" fn  objarray_callback(strdata: *const c_char, nsize: c_uint, index: c_uint) {  
+extern "C" fn  objarray_callback(strdata: *const c_char, nsize: c_uint, _: c_uint) {  
     let n_size: usize = nsize as usize;  
     let bytes = c_str_to_bytes(strdata, n_size); 
     match proto::objectarray::Objectarray::parse_from_bytes(&bytes) {  
@@ -398,7 +507,7 @@ extern "C" fn  objarray_callback(strdata: *const c_char, nsize: c_uint, index: c
         Ok(xobjarray) => {  
             let mut a = DES.lock().unwrap();             
             a.xobjarray = xobjarray;
-            a.nobjupdate = 10;
+            a.nobjupdate = 50; //1 seconds , not valid
         },  
         Err(e) => {  
             eprintln!("Error parsing objarray protobuf: {:?}", e);  
@@ -415,31 +524,32 @@ fn main() {
     let (x,y) = gauss_proj_cal(lon0, lat0);
     println!("x = {} y = {}",x,y);
 
-    let strcommname = "test";  
+    let strcommname = "deciton";  
 
     let c_commname = std::ffi::CString::new(strcommname).unwrap();  
-    let nsize = 100;
-    let npaccount = 10;
+    let nsize: u32 = 100000_u32;
+    let npaccount: u32 = 1_u32;
+
+    let _pa: *mut _ = unsafe { modulecommrust::RegisterSendRUST(c_commname.as_ptr(), nsize, npaccount) };
 
-    let _pa = unsafe { modulecommrust::RegisterSendRUST(c_commname.as_ptr(), nsize, npaccount) };
 
     let strmapmsgname = "newtracemap";
     let c_strmapmsgname = std::ffi::CString::new(strmapmsgname).unwrap();   
-    let _pamap = unsafe { modulecommrust::RegisterRecvRUST(c_strmapmsgname.as_ptr(), tracemap_callback) };
+    let _pamap: *mut _ = unsafe { modulecommrust::RegisterRecvRUST(c_strmapmsgname.as_ptr(), tracemap_callback) };
 
     let strgpsmsgname: &str = "hcp2_gpsimu";
     let c_strgpsmsgname = std::ffi::CString::new(strgpsmsgname).unwrap();   
-    let _pagps = unsafe { modulecommrust::RegisterRecvRUST(c_strgpsmsgname.as_ptr(), gpsimu_callback) };
+    let _pagps: *mut _ = unsafe { modulecommrust::RegisterRecvRUST(c_strgpsmsgname.as_ptr(), gpsimu_callback) };
 
     let strobjmsgname: &str = "lidar_track";
     let c_strobjmsgname = std::ffi::CString::new(strobjmsgname).unwrap();   
-    let _paobj = unsafe { modulecommrust::RegisterRecvRUST(c_strobjmsgname.as_ptr(), objarray_callback) };
+    let _paobj: *mut _ = unsafe { modulecommrust::RegisterRecvRUST(c_strobjmsgname.as_ptr(), objarray_callback) };
 
     loop {
         
-        thread::sleep(Duration::from_millis(1000));
+        thread::sleep(Duration::from_millis(10));
         let mut a = DES.lock().unwrap(); 
-        a.GetDecision();
+        a.get_decision(_pa);
     }
 
  //   unsafe { modulecommrust::UnregisterRUST(_pa) };

src/RUST/rustdecisiondemo/src/modulecommrust.rs

@@ -8,7 +8,9 @@ extern "C" {
 
     pub fn RegisterRecvRUST(strcommname: *const c_char, pCall: extern "C" fn(*const c_char, c_uint, c_uint)) -> *mut c_void;  
 
-    pub fn UnregisterRUST(instance: *mut c_void);  
+    pub fn ModuleSendMsgRUST(instance: *mut c_void, strdata: *const c_char,nDataLen: c_uint);
+
+ //   pub fn UnregisterRUST(instance: *mut c_void);  
 
 
     // ... 其他函数的声明 ...