fusion and decide

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -3705,6 +3705,7 @@ float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
 
 void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gpsMapLine){
     static int obstacle_disable=0;
+    static int speed_slowdown_flag=0;
 
     if(  gpsMapLine[PathPoint]->mode2==3000){
         if(obsDistance>7){       //7   zj-5
@@ -3725,11 +3726,23 @@ void iv::decition::DecideGps00::transferGpsMode2( const std::vector<GPSData> gps
              obstacle_disable=0;
     }else if(gpsMapLine[PathPoint]->mode2==4000){
         ServiceCarStatus.msysparam.vehWidth=5.6;
+    }else if(gpsMapLine[PathPoint]->mode2==5000){
+            speed_slowdown_flag=1;
+    }else if(gpsMapLine[PathPoint]->mode2==5001){
+            speed_slowdown_flag=0;
     }
 
     if(obstacle_disable==1){
             obsDistance=200;
     }
+    if(speed_slowdown_flag==1)
+    {
+            if(realspeed>ServiceCarStatus.mroadmode_vel.mfmode18){
+                minDecelerate=-0.5;
+            }else{
+                dSpeed=min(dSpeed,ServiceCarStatus.mroadmode_vel.mfmode18);
+            }
+    }
 }
 
 float iv::decition::DecideGps00::computeAvoidX(int roadAim,int roadOri,GPSData gps,float vehWidth){

src/detection/detection_lidar_ukf_pda/imm_ukf_pda.cpp

@@ -690,6 +690,12 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
     double tyaw = targets_[i].x_merge_(3);
     double tyaw_rate = targets_[i].x_merge_(4);
 
+//    if(tyaw_rate > 0.1)
+//    {
+//        tyaw_rate = 0.1;
+
+//    }
+
     std::cout<<"          v     tyaw :      "<<tv<<"  "   <<tyaw<<std::endl;
 
     while (tyaw > M_PI)
@@ -697,6 +703,7 @@ void ImmUkfPda::makeOutput(const iv::lidar::objectarray & input,
     while (tyaw < -M_PI)
       tyaw += 2. * M_PI;
 
+
  //   tf::Quaternion q = tf::createQuaternionFromYaw(tyaw);
 
     iv::lidar::lidarobject   dd;

src/fusion/lidar_radar_fusion_cnn/main.cpp

@@ -106,7 +106,7 @@ void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& r
 {
 //    gMutex.lock();
 //    Transformation::RadarPross(radarobjvec);
-//    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
 
     AddMobileye(li_ra_fusion,mobileye_info);
     for(int i=0;i<li_ra_fusion.obj_size();i++)

src/fusion/lidar_radar_fusion_cnn_ukf/fusion.hpp

@@ -158,12 +158,20 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
-        dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
-        dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+        if(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x() > lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()){
+             dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+             dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+        } else {
+            dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+            dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+        }
         dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
         dimension_ = fusion_object.mutable_dimensions();
         dimension_->CopyFrom(dimension);
 
+        std::cout<<" tyaw       angle   "<<lidar_object_arr.obj(match_idx[i].nlidar).angle()<<" __"
+                                          "________________________"<<lidar_object_arr.obj(match_idx[i].nlidar).tyaw()<<std::endl;
+
         if((lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()>0)&&
                 (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
         {
@@ -181,10 +189,10 @@ void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarr
                     float nomal_x = ix*0.2 - xp*0.2;
                     float nomal_y = iy*0.2 - yp*0.2;
                     float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
-                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
-                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
-                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
-                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).angle()) -
+                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).angle());
+                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).angle()) +
+                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).angle());
                     nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
                     nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
                     nomal_centroid_ = fusion_object.add_nomal_centroid();
@@ -262,6 +270,7 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
         iv::fusion::VelXY vel_relative;
         iv::fusion::VelXY *vel_relative_;
         vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
+//        vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x());
         vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
         vel_relative_ = fusion_obj.mutable_vel_relative();
         vel_relative_->CopyFrom(vel_relative);
@@ -276,8 +285,19 @@ void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array,
 
         iv::fusion::Dimension dimension;
         iv::fusion::Dimension *dimension_;
-        dimension.set_x(xobs_info.xobj(j).obswidth());
-        dimension.set_y(2.0);
+        if (xobs_info.xobj(j).obswidth() >= 2.5)
+        {
+          dimension.set_x(2.5);
+          if (xobs_info.xobj(j).obswidth() >= 5.0)
+          {
+            dimension.set_y(5.0);
+          }else {
+             dimension.set_y(xobs_info.xobj(j).obswidth());
+            }
+        }else {
+          dimension.set_x(xobs_info.xobj(j).obswidth());
+          dimension.set_y(2.5);
+        }
         dimension.set_z(1.0);
         dimension_ = fusion_obj.mutable_dimensions();
         dimension_->CopyFrom(dimension);

src/include/proto/fusionobject.proto

@@ -65,7 +65,12 @@ message fusionobject
   repeated PointXYZ point_forecast = 17; //轨迹预测
   repeated NomalForecast forecast_nomals = 18; //yu ce shan ge
   repeated PointXYZ point_historical = 19; //历史轨迹
-  repeated PointXYZI cloud = 20; 
+  repeated PointXYZI cloud = 20;
+  optional int32 behavior_state = 21;
+  optional double velocity_linear_x = 22;
+  optional double acceleration_linear_y = 23;
+  optional bool pose_reliable = 24;
+  optional bool velocity_reliable = 25;
 
 };