fix(lidar,decition):change into pointpillars,add path fix in start position.

src/decition/decition_brain_sf_jsguide/decition/decide_gps_00.cpp

@@ -1199,7 +1199,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     /*==================add logic 20220627===============================*/
-#if 1
+#if 0
    GPS_INS gpsxy;
    gpsxy = Coordinate_UnTransfer(gpsTraceOri[0].x,gpsTraceOri[0].y,now_gps_ins);
    if(GetDistance(gpsxy,now_gps_ins) > 0.5){  //there is distance between car ori_point and first point in trace
@@ -1212,6 +1212,45 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
        gps_decition->speed = 0;
        return gps_decition;
    }
+#else
+//    Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, gpsTraceOri[0]);
+    GPS_INS tempOri = Coordinate_UnTransfer(gpsTraceOri[0].x,gpsTraceOri[0].y,now_gps_ins);
+    double dis = GetDistance(now_gps_ins, tempOri);
+    std::vector<iv::Point2D> trace;
+    trace.resize(300);
+    if(((gpsTraceOri[0].x>1.0)||(gpsTraceOri[0].x<-1.0))&&dis>2&&dis<5)
+    {
+        bool use_new_method = true;
+        if  (use_new_method)
+        {
+            const int val = 300;
+            if(gpsTraceOri.size()>val)
+            {
+                double V = gpsTraceOri[300].y;
+                for (int j = 0; j < val; j++)
+                {
+                    double t = (double)j / val;
+                    double s = t*t*(3.-2.*t);
+                    double ox = s;
+                    double oy = t *( V-3.0)+3.0;
+
+                    trace[j].x=ox*gpsTraceOri[j].x;
+                    trace[j].y=oy;
+                }
+            }
+        }
+
+        for(int j=trace.size()-1;j>=0;j--)
+        {
+//            GPS_INS gpsxy = Coordinate_UnTransfer(trace[j].x,trace[j].y,now_gps_ins);
+            Point2D gpsinxy(trace[j].x,trace[j].y);
+//            vector <char>::iterator theIterator = gpsTraceOri.begin();
+//            gpsTraceOri.insert(gpsTraceOri.begin(),gpsinxy);
+            gpsTraceOri[j] = gpsinxy;
+        }
+        trace.clear();
+    }
+
 #endif
     /*===================================================================*/
 

src/decition/decition_brain_sf_jsguide_new/JS_build_files.txt

@@ -4,3 +4,4 @@ driver_map_xodrload
 ui_ads_hmi
 tool_xodrobj
 decition_brain_sf_jsguide
+decition_brain_sf_jsguide_new

src/detection/detection_lidar_PointPillars_MultiHead/main.cpp

@@ -327,12 +327,12 @@ void PclXYZITToArray(
         //pcl::PointXYZI point = in_pcl_pc_ptr->at(i);
         int indice = indices[i];
         pcl::PointXYZI point = in_pcl_pc_ptr->at(indice);
-        Eigen::Vector3d new_point, old_point;
-        old_point<<point.x, point.y, point.z;
-        new_point = rotation_matrix * (old_point) + trans_matrix;
-        out_points_array[i * 5 + 0] = new_point[0];
-        out_points_array[i * 5 + 1] = new_point[1];
-        out_points_array[i * 5 + 2] = new_point[2];
+//        Eigen::Vector3d new_point, old_point;
+//        old_point<<point.x, point.y, point.z;
+//        new_point = rotation_matrix * (old_point) + trans_matrix;
+        out_points_array[i * 5 + 0] = point.x;
+        out_points_array[i * 5 + 1] = point.y;
+        out_points_array[i * 5 + 2] = point.z;
         out_points_array[i * 5 + 3] =
                 static_cast<float>(point.intensity / normalizing_factor);
         out_points_array[i * 5 + 4] = 0;
@@ -746,13 +746,13 @@ int main(int argc, char *argv[])
                     );
     }
     std::cout<<"PointPillars Init OK."<<std::endl;
-    Eigen::AngleAxisd r_z ( -0.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
-    Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
-    Eigen::AngleAxisd r_x ( -0.0137, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
-    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
+//    Eigen::AngleAxisd r_z ( -0.0418, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 yaw   +
+//    Eigen::AngleAxisd r_y ( -0.0242, Eigen::Vector3d ( 0,1,0 ) ); //沿 Y 轴旋转 roll  +
+//    Eigen::AngleAxisd r_x ( -0.0137, Eigen::Vector3d ( 1,0,0 ) ); //沿 X 轴旋转 pitch -
+//    Eigen::Quaterniond q_zyx = r_z*r_y*r_x; //ZYX旋转顺序（绕旋转后的轴接着旋转）
     // 四元数-->>旋转矩阵
-    rotation_matrix = q_zyx.toRotationMatrix();
-    trans_matrix << 0, 1.25, 0.72;
+//    rotation_matrix = q_zyx.toRotationMatrix();
+//    trans_matrix << 0, 1.25, 0.72;
     gpa = iv::modulecomm::RegisterRecv(gstrinput.data(),ListenPointCloud);
     gpdetect = iv::modulecomm::RegisterSend(gstroutput.data(), 10000000,1);
     gpthread = new std::thread(statethread);

src/driver/driver_lidar_rs16/main.cpp

@@ -179,7 +179,7 @@ int main(int argc, char *argv[])
     showversion("driver_lidar_rs16");
     QCoreApplication a(argc, argv);
 
-    snprintf(gstr_memname,255,"lidar_pc_in");
+    snprintf(gstr_memname,255,"lidar_pc");
     snprintf(gstr_rollang,255,"-9.0");
     snprintf(gstr_inclinationang_xaxis,255,"0.0");
     snprintf(gstr_inclinationang_yaxis,255,"0");