HAPO v2x

src/v2x/v2xTcpClient/gps_type.h

@@ -1,136 +1,131 @@
-#pragma once
-/*
-*GPS 惯导数据
-*/
-#ifndef _IV_COMMON_GPS_TYPE_
-#define _IV_COMMON_GPS_TYPE_
-
-#include <boost.h>
-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 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 = 3.5;			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
-
-        int roadMode;
-        int runMode;
-        int roadSum;
-        int roadOri;
-
-        double mfRoadWidth = 3.5; // Current Road Width
-
-
-
-    };
-
-//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
-    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
-
-   struct Station
-   {
-       int index;
-       GPS_INS station_location;
-       int map_index;
-   };
-
-
-
-     class Point2D
-    {
-      public:
-          double x = 0, y = 0, speed=0;
-         int v1 = 0, v2 = 0;
-         int roadMode = 0;
-
-         Point2D()
-        {
-            x = y = v1 = 0;
-        }
-
-         Point2D(double _x, double _y)
-        {
-            x = _x; y = _y;
-        }
-
-
-     };
-
-
-     class TrafficLight
-    {
-      public:
-         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-
-         TrafficLight()
-        {
-            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
-            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
-        }
-
-     };
-
-     class StationCmd
-     {
-     public:
-         bool received;
-         uint32_t carID,carMode,emergencyStop,stationStop;
-         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
-         uint32_t stationID[20];
-         GPS_INS  stationGps[20];
-         uint32_t stationTotalNum;
-         StationCmd()
-         {
-             received=false;
-             has_carID=false;
-             has_carMode=false;
-             has_emergencyStop=false;
-             has_stationStop=false;
-             mode_manual_drive=false;
-             carID=0;
-             carMode=0;
-             emergencyStop=0;
-             stationStop=0;
-             stationTotalNum=0;
-         }
-     };
-
-
-
-
-}
-#endif // !_IV_COMMON_GPS_TYPE_
+#pragma once
+/*
+*GPS 惯导数据
+*/
+#ifndef _IV_COMMON_GPS_TYPE_
+#define _IV_COMMON_GPS_TYPE_
+
+#include <boost.h>
+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 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
+
+
+
+    };
+
+//    typedef std::shared_ptr<iv::GPS_INS> GPSData;
+    typedef boost::shared_ptr<iv::GPS_INS> GPSData;
+     class Point2D
+    {
+      public:
+          double x = 0, y = 0, speed=0,obs_speed_x=0,obs_speed_y=0;
+         int v1 = 0, v2 = 0;
+         int roadMode = 0;
+         int obs_type=0;
+
+         Point2D()
+        {
+            x = y = v1 = 0;
+        }
+
+         Point2D(double _x, double _y)
+        {
+            x = _x; y = _y;
+        }
+
+
+     };
+
+
+     class TrafficLight
+    {
+      public:
+         int leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+         int leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+
+         TrafficLight()
+        {
+            leftColor = 0, rightColor = 0, straightColor=0, uturnColor=0;
+            leftTime=0, rightTime=0 ,straightTime=0, uturnTime=0;
+        }
+
+     };
+     class StationCmd
+     {
+     public:
+         bool received;
+         uint32_t carID,carMode,emergencyStop,stationStop;
+         bool has_carID,has_carMode,has_emergencyStop,has_stationStop,mode_manual_drive;
+         uint32_t stationID[20];
+         GPS_INS  stationGps[20];
+         uint32_t stationTotalNum;
+         StationCmd()
+         {
+             received=false;
+             has_carID=false;
+             has_carMode=false;
+             has_emergencyStop=false;
+             has_stationStop=false;
+             mode_manual_drive=false;
+             carID=0;
+             carMode=0;
+             emergencyStop=0;
+             stationStop=0;
+             stationTotalNum=0;
+         }
+     };
+
+
+
+
+}
+#endif // !_IV_COMMON_GPS_TYPE_

src/v2x/v2xTcpClient/mainwindow.cpp

@@ -84,11 +84,11 @@ MainWindow::MainWindow(QWidget *parent) :
 
     ModuleFun funlidarObs =std::bind(&MainWindow::UpdateLidarObs,this,std::placeholders::_1, \
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpMemLidarObs = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",funlidarObs);
+    mpMemLidarObs = iv::modulecomm::RegisterRecvPlus("lidar_track",funlidarObs);
 
     ModuleFun funRadarObs =std::bind(&MainWindow::UpdateRadarObs,this,std::placeholders::_1, \
                                    std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpMemRadarObs = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",funRadarObs);
+    mpMemRadarObs = iv::modulecomm::RegisterRecvPlus("radar",funRadarObs);
 
     /* ShareMemory Register END */
 
@@ -492,16 +492,17 @@ QString MainWindow::getObstacleData(int type)
     if(type==0) {
         for(i=0;i<miRadarObsCount;i++) {
             obs=mqRadarObsInfo[i];
-            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon)+","+QString::number(obs.lat)+"]";
+            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon,'f',9)+","+QString::number(obs.lat,'f',9)+"]";
         }
     } else if(type==1) {
         for(i=0;i<miLidarObsCount;i++) {
             obs=mqLidarObsInfo[i];
-            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon)+","+QString::number(obs.lat)+"]";
+            message += ",["+QString::number(obs.type)+","+QString::number(obs.lon,'f',9)+","+QString::number(obs.lat,'f',9)+"]";
         }
     } else {
         ui->textEdit_messages->insertPlainText("ProcessError消息：Obstacle data type get error!!!\n");
     }
+    ui->textEdit_messages->insertPlainText("Obstacle Message: "+message);
     return message;
 }
 
@@ -532,7 +533,7 @@ void MainWindow::UpdateChassis(const char *strdata, const unsigned int nSize, co
     if(xchassis.has_vel())
         mfspeed = xchassis.vel();
     if(xchassis.has_angle_feedback())
-        mfsteerAngle = xchassis.angle_feedback();
+        mfsteerAngle = xchassis.angle_feedback()/870.0*28;
     if((xchassis.has_soc()))
     {
         miSOC = xchassis.soc();
@@ -612,8 +613,9 @@ void MainWindow::UpdateLidarObs(const char * strdata,const unsigned int nSize,co
     {
         x = lidarobjvec.obj(i).centroid().x();
         y = lidarobjvec.obj(i).centroid().y();
+        GaussProjCal(mpdata.gps_lng, mpdata.gps_lat, &mpdata.gps_x, &mpdata.gps_y);
         gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
-        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &(gps_ins.gps_lng), &(gps_ins.gps_lat));
+        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &gps_ins.gps_lng, &gps_ins.gps_lat);
         xobsInfo.lat = gps_ins.gps_lat;
         xobsInfo.lon = gps_ins.gps_lng;
         std::string str = lidarobjvec.obj(i).type_name();
@@ -645,6 +647,8 @@ void MainWindow::UpdateRadarObs(const char * strdata,const unsigned int nSize,co
     oldrecvtime = QDateTime::currentMSecsSinceEpoch();
 
     miRadarObsCount = xradararray.obj_size();
+
+    givlog->verbose("radarobs count %ld",miRadarObsCount);
     ObsInfo xobsInfo;
     double x,y;
     iv::GPS_INS gps_ins;
@@ -652,8 +656,9 @@ void MainWindow::UpdateRadarObs(const char * strdata,const unsigned int nSize,co
     {
         x = xradararray.obj(i).x();
         y = xradararray.obj(i).y();
+        GaussProjCal(mpdata.gps_lng, mpdata.gps_lat, &mpdata.gps_x, &mpdata.gps_y);
         gps_ins = iv::decition::Coordinate_UnTransfer(x, y, mpdata);
-        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &(gps_ins.gps_lng), &(gps_ins.gps_lat));
+        GaussProjInvCal(gps_ins.gps_x, gps_ins.gps_y, &gps_ins.gps_lng, &gps_ins.gps_lat);
         xobsInfo.lat = gps_ins.gps_lat;
         xobsInfo.lon = gps_ins.gps_lng;
         xobsInfo.type = 0;
@@ -661,5 +666,5 @@ void MainWindow::UpdateRadarObs(const char * strdata,const unsigned int nSize,co
     }
     mistRadar = 0;
     mistCanRadar = 0;
-    mistLidar = 0;
+//    mistLidar = 0;
 }