change rustdecisiondemo. not complete.

src/RUST/rustdecisiondemo/src/main.rs

@@ -5,10 +5,12 @@ mod gnssconvert;
 use gnssconvert::gauss_proj_cal;
 
 
-
+use proto::object;
+use proto::objectarray;
 use proto::mapdata::Tracemap;
 use proto::mapdata::Mappoint;
 use proto::gpsimu::{self, Gpsimu};
+use proto::objectarray::Objectarray;
 use protobuf::Message;
 extern  crate protobuf;
 
@@ -17,15 +19,23 @@ use std::thread;
 use std::time::Duration;  
 
 use std::sync::Mutex;  
-use lazy_static::lazy_static;  
+use lazy_static::lazy_static;
+
 
+struct Point2D{
+    mx : f64,
+    my : f64,
+    mhdg : f64
+}
 
   
 struct  Adcdes{
     xtr:Tracemap,
     bxtrupdate : bool,
     xgpsimu: Gpsimu,
-    bgpsimuupdate: bool
+    bgpsimuupdate: bool,
+    xobjarray : Objectarray,
+    nobjupdate : i32
 }
 
 impl Adcdes{
@@ -39,7 +49,8 @@ impl Adcdes{
             return 0.0;
         }
         
-        calcdecisoindemo(&self.xgpsimu,&self.xtr);
+
+        calcdecisoindemo(&self.xgpsimu,&self.xtr,&self.xobjarray,self.nobjupdate);
         
         
         self.bgpsimuupdate = false;
@@ -50,12 +61,12 @@ impl Adcdes{
 lazy_static! {  
 
     static ref DES:Mutex<Adcdes> = Mutex::new(Adcdes {xtr:Tracemap::new(),bxtrupdate : false,
-                xgpsimu:Gpsimu::new(),bgpsimuupdate:false});
+                xgpsimu:Gpsimu::new(),bgpsimuupdate:false,xobjarray:Objectarray::new(),nobjupdate:0});
 
 }  
 
 
-fn FindNearIndex(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
+fn find_near_index(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
     let (x,y) = gauss_proj_cal(xgpsimu.lon(), xgpsimu.lat());
     let mut nearindex = -1;
     let mut neardis = 100000.0;
@@ -83,16 +94,236 @@ fn FindNearIndex(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> i32 {
         index = index + 1
         
     }
-    
+    println!("near index: {}",nearindex);
     nearindex
 }
 
 
-fn calcdecisoindemo(xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> f32{
-    FindNearIndex(xgpsimu, xtracemap);
+fn calcdecisoindemo(xgpsimu:&Gpsimu,xtracemap : &Tracemap,xobjarray : &Objectarray,nobjarrayupdate : i32) -> f32{
+    let mendacc: f64 = -0.7;
+    let nearindex: i32 = find_near_index(xgpsimu, xtracemap);
+    if nearindex<0 {
+        println!("not found near point");
+        return -1.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 endacc: f64 = 0.0;
+    if distoend > 0.1{
+        if distoend < (realspeed/3.6).powf(2.0)/(2.0*(mendacc.powf(2.0)).sqrt()) {
+            endacc = ((realspeed/3.6).powf(2.0)/(2.0*distoend)) *(-1.0)
+        }
+    }
+    else {
+        endacc = mendacc;
+    }
+    
+    if endacc<((-1.0)*1e-9){
+        acc = endacc
+    }
+
+    if acc < -5.0{
+        acc = -5.0
+    }
     1.0
 }
 
+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 mut pd: f64 = realspeed * 0.3;
+    if pd < 4.0 { 
+        pd = 4.0;
+    }
+    
+    let mut sumdis: f64 = 0.0;
+    let mut i: i32 = 1;
+    let size: i32 = localpoints.len() as i32;
+    let mut ppindex: i32 = 0;
+
+    while i < size{
+        let ui: usize = i as usize;
+        sumdis = sumdis + ((localpoints[ui].mx - localpoints[ui-1].mx).powf(2.0) 
+                       + (localpoints[ui].my - localpoints[ui-1].my).powf(2.0)).sqrt();
+        ppindex = i;
+        if sumdis >= pd {
+            ppindex = i;
+            break
+        }
+        i = i+1;
+    }
+    let mut acc = -0.5;
+    let mut wheel = 0.0;
+    if ppindex < 3{
+        acc = 0.0;
+        wheel = 0.0;
+        return (acc,wheel,0.0);
+    }
+
+    if desiredspeed > 0.1{
+    if realspeed < desiredspeed {
+        if realspeed < desiredspeed*0.9{
+            acc = defaultacc;
+            }
+        else{
+            acc = defaultacc *(desiredspeed - realspeed) * (1.0/0.1)/desiredspeed;
+            }
+    }
+    else{
+        if realspeed > desiredspeed*1.1{
+            if realspeed > desiredspeed * 2.0{
+                acc = defaultacc * (-1.0);
+                }
+            else{
+                acc = defaultacc *  (desiredspeed - realspeed)/desiredspeed;
+                }
+            }
+        else {
+            acc = 0.0
+            }
+        }
+    }
+
+    let mut obsdis = 0.0;
+    let mut bhaveobs = false;
+
+    i = 0;
+
+    let mut gridcount = (((mvehwidth/2.0)/gridsize)*2.0) as i32;
+    let xwid = gridcount as f64 * gridsize;
+    if xwid < mvehwidth{
+        gridcount = gridcount +2;
+    }
+
+    while i< size{
+        let ui = 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;
+        while k < nobjsize{
+            let uk = 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;
+            }
+            if bprob == true {
+                break;
+            }
+            k = k+1;
+        }
+        if bprob == false {
+            i = i+1;
+            continue;
+        }
+        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
+            if bhaveobs == true{
+                break
+            }
+            j = j+1
+        }
+        if bhaveobs == true {
+            break
+        }
+        i = i+1;
+    }
+
+    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;
+    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;
+    wheel = (1.0)*kappa * wheel_base * wheelratio * 180.0/std::f64::consts::PI;
+    if wheel>maxwheel{
+        wheel = maxwheel;
+    }
+    if wheel<(maxwheel * (-1.0)){
+        wheel = maxwheel * (-1.0);
+    }
+
+    (acc,wheel,desiredspeed)
+
+}
+
+fn calclocalpath(index:i32,xgpsimu:&Gpsimu,xtracemap : &Tracemap) -> (Vec<Point2D>,f64){
+    let mut localpoints = Vec::new();
+    let mut distoend: f64 = 0.0;
+    let (x0,y0) = gauss_proj_cal(xgpsimu.lon(),xgpsimu.lat());
+    let mut i: i32 = index;
+    let mut npoint: i32 = 0;
+    let nmaxpoint: i32 = 600;
+    let length: i32 = xtracemap.point.len()  as i32;
+    while i < length{
+        let ui: usize = i as usize;
+        let xraw = xtracemap.point[ui].gps_x() - x0;
+        let yraw = xtracemap.point[ui].gps_y() - y0;
+        let mut theta: f64 = (90.0 - xgpsimu.heading()) * std::f64::consts::PI /180.0;
+        let thetaraw: f64 = theta;
+        let pointhead: f64 = (90.0 - xtracemap.point[ui].ins_heading_angle() ) * std::f64::consts::PI/180.0;
+        theta = theta * (-1.0);
+        theta = theta  + std::f64::consts::PI/2.0;
+        let x = xraw * theta.cos() - yraw * theta.sin();
+        let y = xraw * theta.sin() + yraw * theta.cos();
+        localpoints.push(Point2D{mx:x,my:y, mhdg: pointhead - thetaraw  + std::f64::consts::PI/2.0});
+        if i>index {
+            distoend = distoend + ((xtracemap.point[ui].gps_x() - xtracemap.point[ui-1].gps_x()).powf(2.0) 
+                                            + (xtracemap.point[ui].gps_y() - xtracemap.point[ui-1].gps_y()).powf(2.0)).sqrt();
+        }
+        if npoint >= nmaxpoint {
+            break;
+        }
+            
+        i = i+1;
+        npoint = npoint + 1;
+    }
+
+    while i < length {
+        if i>index {
+            let ui = i as usize;
+            distoend = distoend + ((xtracemap.point[ui].gps_x() - xtracemap.point[ui-1].gps_x()).powf(2.0) 
+                                        + (xtracemap.point[ui].gps_y() - xtracemap.point[ui-1].gps_y()).powf(2.0)).sqrt();
+        }
+        i = i+1;
+        if distoend > 300.0 {
+            break;;
+        }
+    }
+
+    (localpoints,distoend)
+}
+
 // 定义Rust中的回调函数，与C/C++中的SMCallBack兼容  
 
 
@@ -159,6 +390,23 @@ 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) {  
+    let n_size: usize = nsize as usize;  
+    let bytes = c_str_to_bytes(strdata, n_size); 
+    match proto::objectarray::Objectarray::parse_from_bytes(&bytes) {  
+
+        Ok(xobjarray) => {  
+            let mut a = DES.lock().unwrap();             
+            a.xobjarray = xobjarray;
+            a.nobjupdate = 10;
+        },  
+        Err(e) => {  
+            eprintln!("Error parsing objarray protobuf: {:?}", e);  
+        },  
+
+    } 
+}
+
 fn main() {
     println!("Hello, world!");
 
@@ -183,6 +431,10 @@ fn main() {
     let c_strgpsmsgname = std::ffi::CString::new(strgpsmsgname).unwrap();   
     let _pagps = 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) };
+
     loop {
         
         thread::sleep(Duration::from_millis(1000));