change view_ndtmatching. add traffic light show.

src/decition/decition_brain_sf_changan_shenlan/decition_brain_sf_changan_shenlan.pro

@@ -106,7 +106,7 @@ HEADERS += \
     ../../include/msgtype/groupmsg.pb.h \
     ../../include/msgtype/brainrunstate.pb.h
 
-DEFINES += USEAUTOWAREMPC
+#DEFINES += USEAUTOWAREMPC
 
 
 if(contains(DEFINES,USEAUTOWAREMPC)){

src/tool/view_ndtmatching/mainwindow.cpp

@@ -15,6 +15,17 @@
 #include <QMessageBox>
 #include <QFile>
 
+
+#include <functional>
+
+using namespace std;
+
+using std::placeholders::_1;
+using std::placeholders::_2;
+using std::placeholders::_3;
+using std::placeholders::_4;
+using std::placeholders::_5;
+
 static pose gCurPose;
 
 static double gView_Z = 100.0;
@@ -120,6 +131,13 @@ void ShowXODR(pcl::visualization::PCLVisualizer& viewer)
 
 pcl::visualization::PCLVisualizer *gpviewer;
 
+static bool mbsetstart = false;
+static bool mbdrawstart = false;
+static bool mbsetend = false;
+static bool mbdrawend = false;
+static double mfstartx,mfstarty;
+static double mfendx,mfendy;
+
 #include <vtkRenderer.h>
 #include <vtkRenderWindow.h>
 
@@ -215,6 +233,12 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
 {
 //    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;
 
+    static double lastx,lasty,lastz,lastyaw;
+    lastx = 0.0;lasty = 0.0; lastz = 0.0; lastyaw = 0.0;
+
+//    int64_t ntime1 = std::chrono::system_clock::now().time_since_epoch().count();
+
+    static int64_t nlastcamup = std::chrono::system_clock::now().time_since_epoch().count();
 
     if(gbCarModelLoad)
     {
@@ -268,6 +292,8 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
     }
     else
     {
+
+
         viewer.removeShape("car");
 
         Eigen::Vector3f trans;
@@ -284,13 +310,17 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
         viewer.addCube(Eigen::Vector3f(gCurPose.x,gCurPose.y,gCurPose.z-1.15),
                        Eigen::Quaternionf(rotation_vector), 4.6, 2.3, 1.5, "car");
 
-
     }
 
     double yaw_calc = M_PI/2.0 - gCurPose.yaw;
 
-    if(gbAutoCam)
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+
+    int64_t nupdiff = nnow - nlastcamup;
+
+    if(gbAutoCam && (abs(nupdiff)> 1e8))
     {
+        nlastcamup = nnow;
         if(gnViewMode == 0)
             viewer.setCameraPosition(gCurPose.x,gCurPose.y,gView_Z,gCurPose.x,gCurPose.y,gCurPose.z,sin(yaw_calc),cos(yaw_calc),0);
 
@@ -368,6 +398,33 @@ void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
 
     }
 
+    double fsesize = 0.5;
+    if(mbsetstart)
+    {
+        if(mbdrawstart)viewer.removeShape("start");
+
+        viewer.addCube(mfstartx -0.5* fsesize,mfstartx + 0.5* fsesize, mfstarty - 0.5* fsesize, mfstarty + 0.5* fsesize,
+                       0,1.5,0.0,1.0,0.0,"start",0);
+
+        mbsetstart = false;
+        mbdrawstart = true;
+    }
+
+    if(mbsetend)
+    {
+        if(mbdrawend)viewer.removeShape("end");
+
+        viewer.addCube(mfendx -0.5* fsesize,mfendx + 0.5* fsesize, mfendy - 0.5* fsesize, mfendy + 0.5* fsesize,
+                       0,1.5,1.0,0.0,0.0,"end",0);
+
+        mbsetend = false;
+        mbdrawend = true;
+    }
+
+//    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+
+//    std::cout<<"view show use: "<<(nnow - ntime1)/1e6<<" ms."<<std::endl;
+
     std::stringstream ss;
  //   viewer.setCameraPosition(mpos_x,0,30,mpos_x + 20,0,0,0,0,0);
  //   ss << "Once per viewer loop: " << count++;
@@ -430,6 +487,10 @@ MainWindow::MainWindow(QWidget *parent) :
     ModuleFun funcnndetect =std::bind(&MainWindow::Updatelidarcnndetect,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpacnndetect  = iv::modulecomm::RegisterRecvPlus("lidar_track",funcnndetect);
 
+    mpamap = iv::modulecomm::RegisterRecvPlus("tracemap",std::bind(&MainWindow::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,
+                                                                   std::placeholders::_4,std::placeholders::_5));
+
+
     connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
     connect(this,SIGNAL(updatedst(double,double)),this,SLOT(onUpdateDst(double,double)));
 
@@ -912,3 +973,60 @@ void MainWindow::onUpdateDst(double w_x, double w_y)
     ui->lineEdit_Lon->setText(QString::number(lon,'f',7));
     ui->lineEdit_Lat->setText(QString::number(lat,'f',7));
 }
+
+void MainWindow::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    if(nSize<sizeof(iv::GPS_INS))
+    {
+        std::cout<<" no map data."<<std::endl;
+        return;
+    }
+
+    int nPointSize = static_cast<int>(nSize/sizeof(iv::GPS_INS));
+    iv::GPS_INS * pmap = (iv::GPS_INS * )strdata;
+
+    double x0,y0;
+    double x,y;
+
+
+    double lon0,lat0;
+    lon0 = mflon0;
+    lat0 = mflat0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(pmap[0].gps_lng,pmap[0].gps_lat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+    mfstartx = x;
+    mfstarty = y;
+    mbsetstart = true;
+
+//    emit signalSetStartPos(x,y);
+
+
+    GaussProjCal(pmap[nPointSize -1].gps_lng,pmap[nPointSize -1].gps_lat,&x,&y);
+    x = x - x0;
+    y = y - y0;
+    mfendx = x;
+    mfendy = y;
+    mbsetend = true;
+
+//    emit signalSetDstPos(x,y);
+
+//    mmutexmap.lock();
+//    mvectormap.clear();
+//    int i;
+//    for(i=0;i<nPointSize;i++)
+//    {
+//        mvectormap.push_back(pmap[i]);
+//    }
+//    mmutexmap.unlock();
+
+//    emit signalUpdateTraceMap();
+
+
+}
+

src/tool/view_ndtmatching/mainwindow.h

@@ -43,6 +43,87 @@ public:
     int lane;
 };
 
+namespace iv {
+struct GPS_INS
+{
+    int valid = 0xff;
+    int index = 0;	//gps点序号
+
+    double gps_lat = 0;//纬度
+    double gps_lng = 0;//经度
+
+    double gps_x = 0;
+    double gps_y = 0;
+    double gps_z = 0;
+
+    double frenet_s=0;
+    double frenet_s_dot=0;
+    double frenet_s_dot_dot=0;
+    double frenet_d=0;
+    double frenet_d_dot=0;
+    double frenet_d_dot_dot=0;
+
+    double ins_roll_angle = 0;	//横滚角 一般定义载体的右、前、上三个方向构成右手系，绕向前的轴旋转就是横滚角，绕向右的轴旋转就是俯仰角，绕向上的轴旋转就是航向角
+    double ins_pitch_angle = 0;	//俯仰角
+    double ins_heading_angle = 0;	//航向角
+
+    int ins_status = 0;	//惯导状态 4
+    int rtk_status = 0;	//rtk状态 6 -5 -3
+    int gps_satelites_num = 0;
+
+    //-----加速度--------------
+    double accel_x = 0;
+    double accel_y = 0;
+    double accel_z = 0;
+
+    //-------角速度------------
+    double ang_rate_x = 0;
+    double ang_rate_y = 0;
+    double ang_rate_z = 0;
+
+    //-----------方向速度--------------
+    double vel_N = 0;
+    double vel_E = 0;
+    double vel_D = 0;
+
+    int speed_mode = 0;
+    int mode2 = 0;
+    double speed = 0;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+    int roadMode;
+    int runMode;
+    int roadSum;
+    int roadOri;
+
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
+
+    bool mbInLaneAvoid = false; //if true suport In Lane Avoid
+    double gps_lat_avoidleft;
+    double gps_lng_avoidleft;
+    double gps_lat_avoidright;
+    double gps_lng_avoidright;
+    double gps_x_avoidleft = 0;
+    double gps_y_avoidleft = 0;
+    double gps_x_avoidright = 0;
+    double gps_y_avoidright = 0;
+
+    bool mbnoavoid = false; //If true this point is no avoid.
+    double mfCurvature = 0.0;
+
+    char mcreserved[10];
+    int mnreserved[5];
+    double mfreserved[2];
+
+
+
+};
+}
+
 namespace Ui {
 class MainWindow;
 }
@@ -88,6 +169,8 @@ private:
 
     void Updatelidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
+    void UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 
     void ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
 
@@ -107,6 +190,7 @@ private:
     void *mpafusiongpsimu;
     void * mpacnndetect;
     void * mpadst;
+    void * mpamap;
 
     pose mCurPose;
 
@@ -130,6 +214,9 @@ private:
     int64_t mnLastFusionGPSIMUUpdate = 0;
     int64_t mnLastNDTUpdate = 0;
 
+
+
+
 private:
     void mouseEventOccurred (const pcl::visualization::MouseEvent &event,
                          void* viewer_void);

src/tool/view_showxodrinvtk/showxodrinvtk.cpp

@@ -75,6 +75,7 @@ void DrawLine(std::vector<iv::LanePoint> & xveclastlp,std::vector<iv::LanePoint>
 void ShowRoad(pcl::visualization::PCLVisualizer& viewer,Road * pRoad,double  fz,int nmode)
 {
 
+    static int index_tra= 0;
     if(pRoad->GetLaneSectionCount()<1)
     {
         qDebug("no lane,return;");
@@ -126,6 +127,42 @@ void ShowRoad(pcl::visualization::PCLVisualizer& viewer,Road * pRoad,double  fz,
         DrawLine(xveclastlp,xveclp,viewer,fz,nmode);
     }
 
+    unsigned int i;
+    unsigned int nsigcount = static_cast<unsigned int>( pRoad->GetSignalCount());
+    for(i=0;i<nsigcount;i++)
+    {
+        Signal * psig = pRoad->GetSignal(i);
+        double s = psig->Gets();
+        double t = psig->Gett();
+        double x,y,hdg;
+        if(pRoad->GetGeometryCoords(s,x,y,hdg)>=0)
+        {
+            double sigx,sigy;
+            sigx = x + t*cos(hdg+M_PI/2.0);
+            sigy = y + t*sin(hdg+M_PI/2.0);
+            double base_size = 0.1;
+            double tra_size = 0.5;
+            double base_height = 5.0;
+            char strbase[100];
+            char strgreen[100];
+            char stryellow[100];
+            char strred[100];
+            snprintf(strbase,100,"base%d",index_tra);
+            snprintf(strgreen,100,"green%d",index_tra);
+            snprintf(stryellow,100,"yellow%d",index_tra);
+            snprintf(strred,100,"red%d",index_tra);
+            index_tra++;
+            viewer.addCube(sigx-base_size/2.0,sigx + base_size/2.0,sigy-base_size/2.0,sigy+base_size/2.0,0,base_height,1.0,1.0,1.0,strbase);
+            viewer.addCube(sigx-tra_size/2.0,sigx + tra_size/2.0,sigy-tra_size/2.0,sigy+tra_size/2.0,base_height,base_height+tra_size,0.0,1.0,0.0,strgreen);
+            viewer.addCube(sigx-tra_size/2.0,sigx + tra_size/2.0,sigy-tra_size/2.0,sigy+tra_size/2.0,base_height + tra_size ,base_height+2.0*tra_size,1.0,1.0,0.0,stryellow);
+            viewer.addCube(sigx-tra_size/2.0,sigx + tra_size/2.0,sigy-tra_size/2.0,sigy+tra_size/2.0,base_height + 2.0*tra_size,base_height+3.0*tra_size,1.0,0.0,0.0,strred);
+        }
+        else
+        {
+            std::cout<<" getcoords fail. signal s "<<s<<std::endl;
+        }
+    }
+