Merge branch 'master' of http://10.14.0.42:3000/adc_pilot/modularization

src/common/modulecomm/shm/procsm.cpp

@@ -247,6 +247,9 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
 
  //       if(!mpASM->attach())
 
+
+
+
         bool bares = mpASM->create(sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize);
         if(bares == false)   //Exist.
         {
@@ -267,7 +270,38 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
             }
             else
             {
-                std::cout<<" Exist,But Fail Attach."<<std::endl;
+                std::cout<<" Exist,But Fail Attach. Recreate "<<std::endl;
+                qint64 uptime = std::chrono::system_clock::now().time_since_epoch().count();
+                snprintf(strasmname,256,"%s_%lld",strsmname,uptime);
+                mpASMPtr->lock();
+                ASM_PTR * pasm = (ASM_PTR *)mpASMPtr->data();
+                pasm->mnshmsize = sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize;
+                pasm->mnUpdateTime = uptime;
+                strncpy(pasm->mstrshmname,strasmname,256);
+                mpASMPtr->unlock();
+                bool bRecreate = mpASM->create(sizeof(procsm_info)+nMaxPacCount*sizeof(procsm_head) + nBufSize);
+                if(bRecreate)
+                {
+                    char * p = (char *)mpASM->data();
+                    if(p == NULL)
+                    {
+                        qDebug("Create SharedMemory Fail.");
+                        return;
+                    }
+                    mpASM->lock();
+                    mpinfo = (procsm_info *)p;
+                    mphead = (procsm_head *)(p+sizeof(procsm_info));
+                    mpinfo->mCap = nMaxPacCount;
+                    mpinfo->mnBufSize = nBufSize;
+                    mpinfo->mFirst = 0;
+                    mpinfo->mNext = 0;
+                    mpinfo->mLock = 0;
+                    mpASM->unlock();
+                }
+                else
+                {
+                    std::cout<<" Recreate Fail. Need Exit Program."<<std::endl;
+                }
             }
 
         }
@@ -621,18 +655,14 @@ int procsm::writemsg(const char *str, const unsigned int nSize)
     }
     mpASM->lock();
 
-
-
-//    unsigned int * pIndexFirst = (unsigned int *)mpASM->data();
-//    unsigned int * pIndexNext = pIndexFirst+1;
-    if(mpinfo->mLock == 1)
-    {
-        std::cout<<"ShareMemory have lock.Init."<<std::endl;
-        mpinfo->mLock = 0;
-        mpinfo->mFirst = 0;
-        mpinfo->mNext = 0;
-    }
-    mpinfo->mLock =1;
+//    if(mpinfo->mLock == 1)
+//    {
+//        std::cout<<"ShareMemory have lock.Init."<<std::endl;
+//        mpinfo->mLock = 0;
+//        mpinfo->mFirst = 0;
+//        mpinfo->mNext = 0;
+//    }
+//    mpinfo->mLock =1;
 WRITEMSG:
     char * pH,*pD;
     QDateTime dt;
@@ -645,6 +675,15 @@ WRITEMSG:
         unsigned int nRemove = mnMaxPacCount/3;
         if(nRemove == 0)nRemove = 1;
         MoveMem(nRemove);
+        if(mnMaxPacCount == 0)
+        {
+            std::cout<<" Warning: mnMaxPacCount == 0,need Recreate."<<std::endl;
+            mpASM->lock();
+            mnMaxPacCount = 1;
+            recreateasm(nSize);
+            return -1;
+        }
+//        std::cout<<" nPac : "<<nPac<<" Max: "<<mnMaxPacCount<<std::endl;
         goto WRITEMSG;
     }
     if(nPac == 0)

src/decition/decition_brain/decition_brain.pro

@@ -18,7 +18,9 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += $$PWD/../common/main.cpp \
     ../../include/msgtype/brainstate.pb.cc \
+    ../../include/msgtype/carstate.pb.cc \
     ../../include/msgtype/decition.pb.cc \
+    ../../include/msgtype/groupmsg.pb.cc \
     ../../include/msgtype/radarobjectarray.pb.cc \
     ../../include/msgtype/radarobject.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
@@ -62,7 +64,9 @@ unix:!macx: DEPENDPATH += $$PWD/.
 
 HEADERS += \
     ../../include/msgtype/brainstate.pb.h \
+    ../../include/msgtype/carstate.pb.h \
     ../../include/msgtype/decition.pb.h \
+    ../../include/msgtype/groupmsg.pb.h \
     ../../include/msgtype/radarobjectarray.pb.h \
     ../../include/msgtype/radarobject.pb.h \
     ../../include/msgtype/gpsimu.pb.h \

src/detection/detection_lidar_cnn_segmentation/detection_lidar_cnn_segmentation.pro

@@ -49,6 +49,8 @@ LIBS += -lpcl_io -lpcl_common
 LIBS += -lboost_system  -lavutil  -lprotobuf -lcudnn
 
 
+LIBS += -lcudnn
+
 unix:!macx: LIBS += -L$$PWD/../../../thirdpartylib/caffe/arm64 -lcaffe -lcudnn
 
 HEADERS += \

src/detection/detection_ndt_matching_gpu_multi/detection_ndt_matching_gpu_multi.pro

@@ -76,6 +76,12 @@ unix:LIBS +=  -lpcl_common\
 
 INCLUDEPATH += $$PWD/../../common/ndt_gpu/
 
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+
 INCLUDEPATH += $$PWD/../../../include/
 LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -lndt_gpu  -livexit -livbacktrace
 

src/driver/driver_gpsimutoodom/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_gpsimutoodom/driver_gpsimutoodom.pro

@@ -0,0 +1,35 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../common/common/math/gnss_coordinate_convert.cpp \
+        ../../include/msgtype/gpsimu.pb.cc \
+        ../../include/msgtype/odom.pb.cc \
+        main.cpp
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+HEADERS += \
+    ../../common/common/math/gnss_coordinate_convert.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/odom.pb.h

src/driver/driver_gpsimutoodom/driver_gpsimutoodom.xml

@@ -0,0 +1,8 @@
+<xml>	
+	<node name="driver_gpsimutoodom">
+		<param name="lon0" value="117.3548316" />
+		<param name="lat0" value="39.0677445" />
+		<param name="gpsmsgname" value="hcp2_gpsimu" />
+		<param name="odommsgname" value="odom" />
+	</node>
+</xml>

src/driver/driver_gpsimutoodom/main.cpp

@@ -0,0 +1,180 @@
+#include <QCoreApplication>
+
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+#include "odom.pb.h"
+#include "gpsimu.pb.h"
+
+#include <iostream>
+#include <memory>
+
+#include "math/gnss_coordinate_convert.h"
+
+#include <cmath>
+
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    Quaternion q;
+    q.w = cy * cp * cr + sy * sp * sr;
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+
+    return q;
+}
+
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
+
+
+
+
+
+static double glon0,glat0,gheight0,ghead0;
+static void * gpa;
+
+static std::string gstrmsg_odom = "odom_ros";
+static std::string gstrmsg_gpsimu = "hcp2_gpsimu";
+
+
+void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,nSize) == false)
+    {
+        std::cout<<" ListenGPSIMU Fail. "<<std::endl;
+        return;
+    }
+
+
+
+    iv::ros::odom xodom;
+    iv::ros::pose_position_def xposition;
+    iv::ros::pose_orientation_def xorientation;
+
+    iv::ros::twist_linear_def xtwist_linear;
+    iv::ros::twist_angular_def xtwist_angular;
+
+
+    double x_o,y_o;
+    double x_now,y_now;
+    GaussProjCal(glon0,glat0,&x_o,&y_o);
+    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&x_now,&y_now);
+    double rel_x = x_now - x_o;
+    double rel_y = y_now - y_o;
+    double rel_z = xgpsimu.height() - gheight0;
+    xposition.set_x(rel_x);
+    xposition.set_y(rel_y);
+    xposition.set_z(rel_z);
+
+    double pitch,roll,yaw;
+    pitch = xgpsimu.pitch() * M_PI/180.0;
+    roll = xgpsimu.roll() * M_PI/180.0;
+    double hdg_o = (90 - ghead0)*M_PI/180.0;
+    yaw = (90 - xgpsimu.heading())*M_PI/180.0  - hdg_o;
+    while(yaw< 0)yaw = yaw +2.0*M_PI;
+    while(yaw>=(2.0*M_PI))yaw = yaw - 2.0*M_PI;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+    xorientation.set_x(quat.x);
+    xorientation.set_y(quat.y);
+    xorientation.set_z(quat.z);
+    xorientation.set_w(quat.w);
+
+    xtwist_linear.set_y(0);
+    xtwist_linear.set_z(0);
+    xtwist_linear.set_x(sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2)));
+
+    xtwist_angular.set_x(0);
+    xtwist_angular.set_y(0);
+    xtwist_angular.set_z(0);
+
+    xodom.mutable_pose_position()->CopyFrom(xposition);
+    xodom.mutable_pose_orientation()->CopyFrom(xorientation);
+    xodom.mutable_twist_linear()->CopyFrom(xtwist_linear);
+    xodom.mutable_twist_angula()->CopyFrom(xtwist_angular);
+
+
+    int nbytesize = xodom.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(xodom.SerializeToArray(str_ptr.get(),nbytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpa,str_ptr.get(),nbytesize);
+    }
+    else
+    {
+        std::cout<<" ListenODOM Serizlize Error."<<std::endl;
+    }
+
+
+}
+
+
+void GetParam(std::string  strpath)
+{
+    iv::xmlparam::Xmlparam xp(strpath);
+    glon0 = xp.GetParam("lon0",117.3548316);
+    glat0 = xp.GetParam("lat0",39.0677445);
+    gstrmsg_gpsimu = xp.GetParam("gpsmsgname","hcp2_gpsimu");
+    gstrmsg_odom = xp.GetParam("odommsgname","odom");
+    ghead0 = 360.0;
+    gheight0 = 0.0;
+}
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+
+    GetParam("./driver_gpsimutoodom.xml");
+
+    gpa = iv::modulecomm::RegisterSend(gstrmsg_odom.data(),10000,1);
+    iv::modulecomm::RegisterRecv(gstrmsg_gpsimu.data(),ListenGPSIMU);
+
+    return a.exec();
+}

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -30,7 +30,8 @@ SOURCES += main.cpp     \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/imu.pb.cc \
     gnss_coordinate_convert.cpp \
-    xodrplan.cpp
+    xodrplan.cpp \
+    ../../include/msgtype/mapdata.pb.cc
 
 HEADERS += \
     ../../../include/ivexit.h \
@@ -44,7 +45,8 @@ HEADERS += \
     ../../include/msgtype/imu.pb.h \
     gnss_coordinate_convert.h \
     planpoint.h \
-    xodrplan.h
+    xodrplan.h \
+    ../../include/msgtype/mapdata.pb.h
 
 
 !include(../../../include/common.pri ) {

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1191,7 +1191,7 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,cons
     pp.y = yold;
     pp.hdg = hdg0;
     pp.dis = 0;
-    xvectorPP.push_back(pp);
+//    xvectorPP.push_back(pp);
 
     double dt = 1.0;
     double tcount = parc->GetLength()/0.1;
@@ -1201,7 +1201,7 @@ static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,cons
     }
     double t;
     s = dt;
-    s = 0.1;
+    s = 0;
 
     double ua,ub,uc,ud,va,vb,vc,vd;
     ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();

src/driver/driver_map_xodrload/main.cpp

@@ -12,6 +12,8 @@
 
 #include "gpsimu.pb.h"
 
+#include "mapdata.pb.h"
+
 #include "v2x.pb.h"
 
 #include "modulecomm.h"
@@ -48,6 +50,7 @@ void * gpmap;
 void * gpagps;
 void * gpasimple;
 void * gpav2x;
+static void * gpanewtrace;
 
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
@@ -508,6 +511,8 @@ void SetPlan(xodrobj xo)
     }
     
 
+   iv::map::tracemap xtrace;
+
     nSize = xPlan.size();
 
     std::vector<iv::GPSData> mapdata;
@@ -529,19 +534,78 @@ void SetPlan(xodrobj xo)
 
     for(i=0;i<nSize;i++)
     {
-        iv::GPSData data(new iv::GPS_INS);
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-                   data->gps_lng,data->ins_heading_angle);
+        iv::map::mappoint * pmappoint =  xtrace.add_point();
+        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
         CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
-                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
+                   gps_lat_avoidleft,gps_lon_avoidleft);
         CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
-                   data->gps_lat_avoidright,data->gps_lng_avoidright);
+                   gps_lat_avoidright,gps_lon_avoidright);
+
+        pmappoint->set_gps_lat(gps_lat);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
+        pmappoint->set_gps_lng(gps_lon);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
+        pmappoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
+
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
+        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
+
+        pmappoint->set_gps_x(gps_x);
+        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
+        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
+        pmappoint->set_gps_y(gps_y);
+        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
+        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
+
+        pmappoint->set_speed(xPlan[i].speed);
+        pmappoint->set_index(i);
+
+        pmappoint->set_roadori(xPlan[i].mnLaneori);
+        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
+        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
+        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
+        pmappoint->set_mflanewidth(xPlan[i].mWidth);
+        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
+        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
+
+
+        iv::GPSData data(new iv::GPS_INS);
+        data->gps_lat = gps_lat;
+        data->gps_lat_avoidleft = gps_lat_avoidleft;
+        data->gps_lat_avoidright = gps_lat_avoidright;
+        data->gps_lng = gps_lon;
+        data->gps_lng_avoidleft = gps_lon_avoidleft;
+        data->gps_lng_avoidright =  gps_lon_avoidright;
+        data->ins_heading_angle = gps_heading;
+        data->gps_x = gps_x;
+        data->gps_x_avoidleft = gps_x_avoidleft;
+        data->gps_x_avoidright = gps_x_avoidright;
+        data->gps_y = gps_y;
+        data->gps_y_avoidleft = gps_y_avoidleft;
+        data->gps_y_avoidright = gps_y_avoidright;
+        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
+        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
+
+
+//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+//                   data->gps_lng,data->ins_heading_angle);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
         data->index = i;
         data->speed = xPlan[i].speed;
  //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
-        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
+//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
+//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
 
         givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
@@ -560,8 +624,11 @@ void SetPlan(xodrobj xo)
         {
             data->roadOri = 0;
             data->roadSum = 2;
+            pmappoint->set_roadori(0);
+            pmappoint->set_roadsum(2);
         }
 
+
         data->mbnoavoid = xPlan[i].mbNoavoid;
         if(data->mbnoavoid)
         {
@@ -597,6 +664,14 @@ void SetPlan(xodrobj xo)
             }
          }
 
+       pmappoint->set_mode2(data->mode2);
+       pmappoint->set_rel_x(xPlan[i].x);
+       pmappoint->set_rel_y(xPlan[i].y);
+       pmappoint->set_rel_z(0);
+       pmappoint->set_rel_yaw(xPlan[i].hdg);
+       pmappoint->set_laneid(-1);
+       pmappoint->set_roadid(xPlan[i].nRoadID);
+
 #ifdef BOAVOID
     if(isboringroad(xPlan[i].nRoadID))
     {
@@ -633,14 +708,9 @@ void SetPlan(xodrobj xo)
     }
 #endif
 
-//        data->roadSum = 1;
-//        data->roadMode = 0;
-//        data->roadOri = 0;
+        mapdata.push_back(data);
 
-//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
 
-        mapdata.push_back(data);
 
 
 //        char strline[255];
@@ -669,6 +739,17 @@ void SetPlan(xodrobj xo)
     xfile_1.close();
     ShareMap(mapdata);
 
+    int nnewmapsize = xtrace.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
+    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
+    }
+    else
+    {
+        std::cout<<" new trace map serialize fail."<<std::endl;
+    }
+
 
 
 
@@ -961,12 +1042,16 @@ int main(int argc, char *argv[])
     LoadXODR(strmapth);
 
     gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
+    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
+    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
+
+
     gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
     gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
     gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
     gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
 
-    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
+
 
 
     double x_src,y_src,x_dst,y_dst;

src/driver/driver_odomtogpsimu/main.cpp

@@ -70,6 +70,12 @@ Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch
 static double glon0,glat0,gheight0,ghead0;
 static void * gpa;
 
+static std::string gstrmsg_odom = "odom_ros";
+static std::string gstrmsg_gpsimu = "hcp2_gpsimu";
+
+static double gfix_x = 0;
+static double gfix_y = 0;
+
 
 void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -105,6 +111,17 @@ void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int
     double hdg_o = (90 - ghead0)*M_PI/180.0;
     double rel_x = xodom.pose_position().x() * cos(hdg_o) - xodom.pose_position().y() * sin(hdg_o);
     double rel_y = xodom.pose_position().x() * sin(hdg_o) + xodom.pose_position().y() * cos(hdg_o);
+
+    double fix_hdg = 90;//104.824;
+    double fix_x = gfix_x;//-13.0;
+    double fix_y = gfix_y; //4.0;
+    double hdg_fix = (90 - fix_hdg)*M_PI/180.0;
+    double rel_fix_x = fix_x * cos(hdg_fix) - fix_y * sin(hdg_fix);
+    double rel_fix_y = fix_x * sin(hdg_fix) + fix_y * cos(hdg_fix);
+
+    rel_x = rel_x + rel_fix_x;
+    rel_y = rel_y + rel_fix_y;
+
     GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
     double hdg_c = hdg_o + xAng.yaw;
 
@@ -156,6 +173,10 @@ void GetParam(std::string  strpath)
     iv::xmlparam::Xmlparam xp(strpath);
     glon0 = xp.GetParam("lon0",117.3548316);
     glat0 = xp.GetParam("lat0",39.0677445);
+    gstrmsg_gpsimu = xp.GetParam("gpsmsgname","hcp2_gpsimu");
+    gstrmsg_odom = xp.GetParam("odommsgname","odom_ros");
+    gfix_x = xp.GetParam("fix_x",-13.0);
+    gfix_y = xp.GetParam("fix_y",4.0);
     ghead0 = 360.0;
     gheight0 = 0.0;
 }
@@ -164,10 +185,10 @@ int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    GetParam("./driver_odomtogpsimu");
+    GetParam("./driver_odomtogpsimu.xml");
 
-    gpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",10000,1);
-    iv::modulecomm::RegisterRecv("odom_ros",ListenODOM);
+    gpa = iv::modulecomm::RegisterSend(gstrmsg_gpsimu.data(),10000,1);
+    iv::modulecomm::RegisterRecv(gstrmsg_odom.data(),ListenODOM);
 
 
     return a.exec();

src/include/proto/mapdata.proto

@@ -0,0 +1,64 @@
+syntax = "proto2";
+
+package iv.map;
+
+message mappoint
+{
+        optional double gps_lat = 1 [default=0.0];//纬度
+        optional double gps_lng  = 2 [default=0.0];//经度
+
+        optional double gps_x  = 3 [default=0];   
+        optional double gps_y  = 4 [default=0];
+        optional double gps_z  = 5 [default=0];
+
+        optional double ins_heading_angle  = 6 [default=0];	//航向角
+
+
+        optional int32 speed_mode  = 7 [default=0];
+        optional int32 mode2  = 8 [default=0];
+        optional double speed  = 9 [default=0];			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        optional int32 roadMode = 10[default=0];
+        optional int32 runMode = 11[default=0];
+        optional int32 roadSum = 12[default=1];
+        optional int32 roadOri = 13[default=0];
+
+        optional double mfLaneWidth  = 14 [default=3.5]; // Current Lane Width
+
+        optional double mfDisToLaneLeft  = 15 [default=1.8]; //Distance to Lane Left
+        optional int32 mnLaneChangeMark  = 16 [default=0]; //1 to Left 0 not change   -1 to right
+        optional double mfDisToRoadLeft  = 17 [default=1.8]; //Distance to Road Left
+        optional double mfRoadWidth  = 18 [default=3.5]; // Road Width
+
+        optional bool mbInLaneAvoid  = 19 [default=true]; //if true suport In Lane Avoid
+        optional  double gps_lat_avoidleft = 20;
+        optional double gps_lng_avoidleft = 21;
+        optional double gps_lat_avoidright = 22;
+        optional double gps_lng_avoidright = 23;
+        optional double gps_x_avoidleft = 24;
+        optional double gps_y_avoidleft = 25;
+        optional double gps_x_avoidright = 26;
+        optional double gps_y_avoidright = 27;
+
+        optional bool mbnoavoid  = 28 [default=false]; //If true this point is no avoid.
+        optional double mfCurvature  = 29 [default=0];
+	
+	optional double rel_x = 30;  //relative x
+	optional double rel_y = 31;   //relative y
+	optional double rel_z = 32;   //relative z
+	optional double rel_yaw = 33;  //yaw
+	optional int32 laneid = 34;  // -1 -2 -3 right lane   1 2 3 left lane
+	optional int32 leftlanecout = 35; 
+	optional int32 rightlanecount = 36;
+	optional int32 roadid = 37;
+	optional int32 index = 38;
+
+
+};
+
+
+message tracemap
+{
+	repeated mappoint point = 1;
+	optional int32 nState  = 2;  
+};

src/include/proto/maptype.proto

@@ -65,6 +65,9 @@ message maptype
 
         optional bool mbnoavoid  = 44 [default=false]; //If true this point is no avoid.
         optional double mfCurvature  = 45 [default=0];
+	
 
 
 };
+
+

src/ros/catkin/src/lgsvl_msgs-master/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package lgsvl_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.0.4 (2020-10-21)
+------------------
+* Add VehicleOdometry.msg to README
+* Adding VehicleOdometry.
+* Update README and LICENSE
+* Update package description and maintainers
+* Add ultrasonic message.
+* Contributors: Guodong Rong, Hadi Tabatabaee, Piotr Jaroszek
+
+0.0.3 (2020-05-29)
+------------------
+* Update readme and license
+* Changing some Vector3's to Points where they make more sense.
+* Renaming CanBus to CanBusData.
+* Adding VehicleStateData.
+* Adding VehicleControlData.
+* Adding CanBus and DetectedRadarObject/Array.
+* Making package hybrid ROS1/ROS2 package.
+* Contributors: Hadi Tabatabaee, Joshua Whitley
+
+0.0.2 (2020-03-04)
+------------------
+* Include signal messages in README
+* add messages for ground truth signals
+* updated license
+* Contributors: David Uhm
+
+0.0.1 (2018-12-18)
+------------------
+* add changelog
+* update initial version number
+* add license
+* add Ros package for ground truth messages
+* initial commit
+* Contributors: David Uhm

src/ros/catkin/src/lgsvl_msgs-master/CMakeLists.txt

@@ -0,0 +1,104 @@
+project(lgsvl_msgs)
+
+find_package(ros_environment REQUIRED)
+
+set(ROS_VERSION $ENV{ROS_VERSION})
+
+set(MSG_FILES
+  "BoundingBox2D.msg"
+  "BoundingBox3D.msg"
+  "CanBusData.msg"
+  "DetectedRadarObjectArray.msg"
+  "DetectedRadarObject.msg"
+  "Detection2D.msg"
+  "Detection2DArray.msg"
+  "Detection3D.msg"
+  "Detection3DArray.msg"
+  "Signal.msg"
+  "SignalArray.msg"
+  "Ultrasonic.msg"
+  "VehicleControlData.msg"
+  "VehicleStateData.msg"
+  "VehicleOdometry.msg"
+)
+
+if(${ROS_VERSION} EQUAL 1)
+  cmake_minimum_required(VERSION 2.8.3)
+
+  find_package(catkin REQUIRED COMPONENTS
+    geometry_msgs
+    message_generation
+    sensor_msgs
+    std_msgs
+  )
+
+  # Default to C++11
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD 11)
+  endif()
+
+  add_message_files(
+    DIRECTORY msg
+    FILES ${MSG_FILES}
+  )
+
+  generate_messages(
+    DEPENDENCIES
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+  )
+
+  catkin_package(
+    CATKIN_DEPENDS
+    message_runtime
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+  )
+elseif(${ROS_VERSION} EQUAL 2)
+  cmake_minimum_required(VERSION 3.5)
+
+  find_package(ament_cmake REQUIRED)
+  find_package(builtin_interfaces REQUIRED)
+  find_package(geometry_msgs REQUIRED)
+  find_package(sensor_msgs REQUIRED)
+  find_package(std_msgs REQUIRED)
+  find_package(rosidl_default_generators REQUIRED)
+
+  # Default to C++14
+  if(NOT CMAKE_CXX_STANDARD)
+    set(CMAKE_CXX_STANDARD_REQUIRED ON)
+    set(CMAKE_CXX_STANDARD 14)
+  endif()
+
+  if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+  endif()
+
+  # Apend "msg/" to each file name
+  set(TEMP_LIST "")
+  foreach(MSG_FILE ${MSG_FILES})
+    list(APPEND TEMP_LIST "msg/${MSG_FILE}")
+  endforeach()
+  set(MSG_FILES ${TEMP_LIST})
+
+  rosidl_generate_interfaces(${PROJECT_NAME}
+    ${MSG_FILES}
+    DEPENDENCIES
+      builtin_interfaces
+      geometry_msgs
+      sensor_msgs
+      std_msgs
+    ADD_LINTER_TESTS
+  )
+
+  ament_export_dependencies(rosidl_default_runtime)
+
+  if(BUILD_TESTING)
+    find_package(ament_lint_auto REQUIRED)
+    ament_lint_auto_find_test_dependencies()
+  endif()
+
+  ament_package()
+endif()

src/ros/catkin/src/lgsvl_msgs-master/LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2019-2020, LG Electronics
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of LG Electronics nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

src/ros/catkin/src/lgsvl_msgs-master/README.md

@@ -0,0 +1,25 @@
+# ROS Package lgsvl_msgs for LG SVL Automotive Simulator
+
+This repository contains ROS message definitions for lgsvl_msgs to subscribe ROS messages being published by LG SVL Automotive Simulator via rosbridge.
+
+```text
+  - Detection3DArray.msg    # A list of 3D detections
+  - Detection3D.msg         # 3D detection including id, label, score, and 3D bounding box
+  - BoundingBox3D.msg       # A 3D bounding box definition
+  - Detection2DArray.msg    # A list of 2D detections
+  - Detection2D.msg         # 2D detection including id, label, score, and 2D bounding box
+  - BoundingBox2D.msg       # A 2D bounding box definition
+  - SignalArray.msg         # A list of traffic light detections
+  - Signal.msg              # 3D detection of a traffic light including id, label, score, and 3D bounding box
+  - CanBusData.msg          # Can bus data for an ego vehicle published by the simulator
+  - VehicleControlData.msg  # Vehicle control commands that the simulator subscribes to
+  - VehicleStateData.msg    # Description of the full state of an ego vehicle
+  - Ultrasonic.msg          # Minimum detected distance by an ultrasonic sensor
+  - VehicleOdometry.msg     # Odometry for an ego vehicle
+```
+
+## Copyright and License
+
+Copyright (c) 2018-2020 LG Electronics, Inc.
+
+This software contains code licensed as described in LICENSE.

src/ros/catkin/src/lgsvl_msgs-master/msg/BoundingBox2D.msg

@@ -0,0 +1,4 @@
+float32 x  # x position of the bounding box center in camera image space, in pixels
+float32 y  # y position of the bounding box center in camera image space, in pixels
+float32 width  # width of the bounding box, in pixels
+float32 height  # height of the bounding box, in pixels

src/ros/catkin/src/lgsvl_msgs-master/msg/BoundingBox3D.msg

@@ -0,0 +1,2 @@
+geometry_msgs/Pose position  # 3D position and orientation of the bounding box center in Lidar space, in meters
+geometry_msgs/Vector3 size  # Size of the bounding box, in meters

src/ros/catkin/src/lgsvl_msgs-master/msg/CanBusData.msg

@@ -0,0 +1,29 @@
+std_msgs/Header header
+
+float32 speed_mps
+float32 throttle_pct  # 0 to 1
+float32 brake_pct     # 0 to 1
+float32 steer_pct     # -1 to 1
+bool parking_brake_active
+bool high_beams_active
+bool low_beams_active
+bool hazard_lights_active
+bool fog_lights_active
+bool left_turn_signal_active
+bool right_turn_signal_active
+bool wipers_active
+bool reverse_gear_active
+int8 selected_gear
+bool engine_active
+float32 engine_rpm
+float64 gps_latitude
+float64 gps_longitude
+float64 gps_altitude
+geometry_msgs/Quaternion orientation
+geometry_msgs/Vector3 linear_velocities
+
+int8 GEAR_NEUTRAL = 0
+int8 GEAR_DRIVE = 1
+int8 GEAR_REVERSE = 2
+int8 GEAR_PARKING = 3
+int8 GEAR_LOW = 4

src/ros/catkin/src/lgsvl_msgs-master/msg/DetectedRadarObject.msg

@@ -0,0 +1,18 @@
+int32 id
+
+geometry_msgs/Vector3 sensor_aim
+geometry_msgs/Vector3 sensor_right
+geometry_msgs/Point sensor_position
+geometry_msgs/Vector3 sensor_velocity
+float64 sensor_angle
+
+geometry_msgs/Point object_position
+geometry_msgs/Vector3 object_velocity
+geometry_msgs/Point object_relative_position
+geometry_msgs/Vector3 object_relative_velocity
+geometry_msgs/Vector3 object_collider_size
+uint8 object_state
+bool new_detection
+
+uint8 STATE_MOVING = 0
+uint8 STATE_STATIONARY = 1

src/ros/catkin/src/lgsvl_msgs-master/msg/DetectedRadarObjectArray.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+
+lgsvl_msgs/DetectedRadarObject[] objects

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection2D.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected object
+string label  # car, pedestrian
+float32 score  # The confidence score of the detected object in the range [0-1]
+BoundingBox2D bbox  # A 2D bounding box
+geometry_msgs/Twist velocity  # Linear and angular velocity

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection2DArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Detection2D[] detections  # A list of 2D detections

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection3D.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected object
+string label  # car, pedestrian
+float32 score  # The confidence score of the detected object in the range [0-1]
+BoundingBox3D bbox  # A 3D bounding box
+geometry_msgs/Twist velocity  # Linear and angular velocity

src/ros/catkin/src/lgsvl_msgs-master/msg/Detection3DArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Detection3D[] detections  # A list of 3D detections

src/ros/catkin/src/lgsvl_msgs-master/msg/Signal.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+uint32 id  # The numeric ID of the detected signal
+string label  # green, yellow, red
+float32 score  # The confidence score of the detected signal in the range [0-1]
+BoundingBox3D bbox  # A 3D bounding box

src/ros/catkin/src/lgsvl_msgs-master/msg/SignalArray.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+Signal[] signals  # A list of traffic signals

src/ros/catkin/src/lgsvl_msgs-master/msg/Ultrasonic.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+float32 minimum_distance

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleControlData.msg

@@ -0,0 +1,13 @@
+std_msgs/Header header
+
+float32 acceleration_pct  # 0 to 1
+float32 braking_pct  # 0 to 1
+float32 target_wheel_angle  # radians
+float32 target_wheel_angular_rate  # radians / second
+uint8 target_gear
+
+uint8 GEAR_NEUTRAL = 0
+uint8 GEAR_DRIVE = 1
+uint8 GEAR_REVERSE = 2
+uint8 GEAR_PARKING = 3
+uint8 GEAR_LOW = 4

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleOdometry.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+float32 velocity # meters / second 
+float32 front_wheel_angle # radians
+float32 rear_wheel_angle # radians

src/ros/catkin/src/lgsvl_msgs-master/msg/VehicleStateData.msg

@@ -0,0 +1,36 @@
+std_msgs/Header header
+
+uint8 blinker_state
+uint8 headlight_state
+uint8 wiper_state
+uint8 current_gear
+uint8 vehicle_mode
+bool hand_brake_active
+bool horn_active
+bool autonomous_mode_active
+
+uint8 BLINKERS_OFF = 0
+uint8 BLINKERS_LEFT = 1
+uint8 BLINKERS_RIGHT = 2
+uint8 BLINKERS_HAZARD = 3
+
+uint8 HEADLIGHTS_OFF = 0
+uint8 HEADLIGHTS_LOW = 1
+uint8 HEADLIGHTS_HIGH = 2
+
+uint8 WIPERS_OFF = 0
+uint8 WIPERS_LOW = 1
+uint8 WIPERS_MED = 2
+uint8 WIPERS_HIGH = 3
+
+uint8 GEAR_NEUTRAL = 0
+uint8 GEAR_DRIVE = 1
+uint8 GEAR_REVERSE = 2
+uint8 GEAR_PARKING = 3
+uint8 GEAR_LOW = 4
+
+uint8 VEHICLE_MODE_COMPLETE_MANUAL = 0
+uint8 VEHICLE_MODE_COMPLETE_AUTO_DRIVE = 1
+uint8 VEHICLE_MODE_AUTO_STEER_ONLY = 2
+uint8 VEHICLE_MODE_AUTO_SPEED_ONLY = 3
+uint8 VEHICLE_MODE_EMERGENCY_MODE = 4

src/ros/catkin/src/lgsvl_msgs-master/package.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lgsvl_msgs</name>
+  <version>0.0.4</version>
+  <description>Message definitions for interfacing with the LGSVL Simulator for ROS and ROS 2.</description>
+  <maintainer email="hadi.tabatabaee@lge.com">Hadi Tabatabaee</maintainer>
+  <maintainer email="david.uhm@lge.com">David Uhm</maintainer>
+  <license>BSD</license>
+  <author email="david.uhm@lge.com">David Uhm</author>
+
+  <buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
+  <buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
+
+  <build_depend condition="$ROS_VERSION == 1">message_generation</build_depend>
+  <build_depend condition="$ROS_VERSION == 2">rosidl_default_generators</build_depend>
+  <build_depend>ros_environment</build_depend>
+
+  <depend condition="$ROS_VERSION == 2">builtin_interfaces</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <exec_depend condition="$ROS_VERSION == 1">message_runtime</exec_depend>
+  <exec_depend condition="$ROS_VERSION == 2">rosidl_default_runtime</exec_depend>
+
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_auto</test_depend>
+  <test_depend condition="$ROS_VERSION == 2">ament_lint_common</test_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type condition="$ROS_VERSION == 1">catkin</build_type>
+    <build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
+  </export>
+</package>

src/ros/catkin/src/pilottoros/CMakeLists.txt

@@ -6,14 +6,16 @@ message(STATUS  "Enter pilottoros")
 
 
 SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
+SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)
 
-
-find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore)
+find_path(QT_EXIST   QtCore   ${QT_PATH}/include/QtCore  ${AGX_QT_PATH}/QtCore)
 
 if(QT_EXIST)
 include_directories(
   ${QT_PATH}/include
   ${QT_PATH}/include/QtCore
+  ${AGX_QT_PATH}
+  ${AGX_QT_PATH}/QtCore
 )
 link_directories(
   ${QT_PATH}/lib
@@ -119,6 +121,6 @@ include_directories(
 )
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp src  ./../../../../include/msgtype/rawpic.pb.cc )
+add_executable(${PROJECT_NAME}_node src/main.cpp src  ./../../../../include/msgtype/rawpic.pb.cc ./../../../../include/msgtype/odom.pb.cc)
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}   Qt5Core Qt5Xml modulecomm)
 

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -13,7 +13,10 @@
 #include<sensor_msgs/image_encodings.h>
 #include<image_transport/image_transport.h>
 
+#include <nav_msgs/Odometry.h>
+
 #include "rawpic.pb.h"
+#include "odom.pb.h"
 
 
 static std::string _point_topic = "/points_raw";
@@ -22,8 +25,13 @@ static std::string _point_msgname = "lidar_pc";
 static std::string _image_topic = "/image_raw";
 static std::string _image_msgname = "picfront";
 
+static std::string  _odom_topic = "/odom";
+static std::string _odom_msgname = "odom";
+
 ros::Publisher  point_cloud_pub;
 image_transport::Publisher  image_pub;
+ros::Publisher odom_pub;
+
 
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -90,7 +98,7 @@ void Listenpic(const char * strdata,const unsigned int nSize,const unsigned int
     else
     {
         std::vector<unsigned char> buff(pic.picdata().data(),pic.picdata().data()+pic.picdata().size());
-        mat = cv::imdecode(buff,CV_LOAD_IMAGE_COLOR);
+        mat = cv::imdecode(buff,1);
     }
 
     sensor_msgs::ImagePtr msg;
@@ -100,13 +108,47 @@ void Listenpic(const char * strdata,const unsigned int nSize,const unsigned int
 
 }
 
+void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::ros::odom xodom;
+    if(false == xodom.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"Listenodom parse fail."<<std::endl;
+        return;
+    }
+
+    nav_msgs::Odometry msg;
+
+    msg.pose.pose.position.x = xodom.pose_position().x();
+    msg.pose.pose.position.y = xodom.pose_position().y();
+    msg.pose.pose.position.z = xodom.pose_position().z();
+
+    msg.pose.pose.orientation.x = xodom.pose_orientation().x();
+    msg.pose.pose.orientation.y = xodom.pose_orientation().y();
+    msg.pose.pose.orientation.z = xodom.pose_orientation().z();
+    msg.pose.pose.orientation.w = xodom.pose_orientation().w();
+
+    msg.twist.twist.linear.x = xodom.twist_linear().x();
+    msg.twist.twist.linear.y = xodom.twist_linear().y();
+    msg.twist.twist.linear.z = xodom.twist_linear().z();
+
+    msg.twist.twist.angular.x = xodom.twist_angula().x();
+    msg.twist.twist.angular.y = xodom.twist_angula().y();
+    msg.twist.twist.angular.z = xodom.twist_angula().z();
+
+
+  odom_pub.publish(msg);
+
+}
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "pilottoros");
-	ros::NodeHandle n;
+//	ros::NodeHandle n;
+    ros::NodeHandle private_nh("~");
 	ros::Rate loop_rate(1);
 
-	    ros::NodeHandle private_nh("~");
+	    
     private_nh.getParam("points_node",_point_topic);
 	private_nh.getParam("points_msgname",_point_msgname);
     std::cout<<"_point_topic is "<<_point_topic<<std::endl;
@@ -117,16 +159,21 @@ int main(int argc, char *argv[])
     std::cout<<"_image_topic is "<<_image_topic<<std::endl;
 	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
 
-    point_cloud_pub = n.advertise<sensor_msgs::PointCloud2>(
+    point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
                 _point_topic, 10);
-	void * pliar = iv::modulecomm::RegisterRecv(_point_msgname.data(),ListenPointCloud);
+	
 
 
     // 定义节点句柄   
-    image_transport::ImageTransport it(n);
+    image_transport::ImageTransport it(private_nh);
     image_pub  = it.advertise("image_raw", 1);
 
+    odom_pub =  private_nh.advertise<nav_msgs::Odometry>(_odom_topic,10);
+
     void * pimage = iv::modulecomm::RegisterRecv(_image_msgname.data(),Listenpic);
+    void * pliar = iv::modulecomm::RegisterRecv(_point_msgname.data(),ListenPointCloud);
+    void * podom = iv::modulecomm::RegisterRecv(_odom_msgname.data(),Listenodom);
+
 
 
 ros::spin();

src/ros/catkin/src/rostopilot/CMakeLists.txt

@@ -42,6 +42,11 @@ if (MODULECOMM_INCLUDE_DIR  AND  MODULECOMM_LIBRARAY_DIR )
 endif ()
 
 
+
+find_package(autoware_msgs REQUIRED)
+find_package(lgsvl_msgs REQUIRED)
+
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -53,6 +58,8 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   pcl_ros
   pcl_conversions
+  autoware_msgs
+  lgsvl_msgs
 )
 find_package(Boost REQUIRED)
 find_package(OpenCV REQUIRED)
@@ -116,13 +123,15 @@ catkin_package(
 include_directories(
 ##  INCLUDE_DIRS include
   ${CMAKE_CURRENT_BINARY_DIR}/..
-  ${CMAKE_SOURCE_DIR}/../devel/include
+ # ${CMAKE_SOURCE_DIR}/../devel/include
   ${catkin_INCLUDE_DIRS} include
   ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
+  ${lgsvl_msgs_INCLUDE_DIRS}
 )
 
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc   ./../../../../include/msgtype/odom.pb.cc  )
+add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc   ./../../../../include/msgtype/odom.pb.cc  ./../../../../include/msgtype/decition.pb.cc )
 target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}  ${OpenCV_LIBRARIES}    ${Protobuf_LIBRARIES}  Qt5Core Qt5Xml modulecomm)
 

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -19,9 +19,12 @@
 #include <nav_msgs/Odometry.h>
 
 #include  "autoware_msgs/VehicleCmd.h"
+#include "lgsvl_msgs/VehicleControlData.h"
+#include "lgsvl_msgs/VehicleStateData.h"
 
 #include "rawpic.pb.h"
 #include "odom.pb.h"
+#include "decition.pb.h"
 
 static std::string _point_topic = "/points_raw";
 static std::string _point_msgname = "lidarpc_ros";
@@ -42,6 +45,8 @@ static std::string _odom_msgname = "odom_ros";
 
 
 ros::Publisher  test_cmd_pub;
+ros::Publisher lgsvl_state_pub;
+ros::Publisher lgsvl_cmd_pub;
 
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
@@ -72,6 +77,10 @@ static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 }
 
 
+#ifndef CV_IMWRITE_JPEG_QUALITY
+#define CV_IMWRITE_JPEG_QUALITY 1
+#endif
+
 int gindex = 0;
 int gcompress = 1;
 void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
@@ -178,23 +187,85 @@ void testvh()
      int x = 0;
      while(1)
      {
-         autoware_msgs::VehicleCmd xcmd;
 
-         xcmd.header.seq = x;
+         lgsvl_msgs::VehicleStateData xstate;
+         xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
+         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_HIGH;
+         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_PARKING;
+         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+         xstate.hand_brake_active = true;
+         xstate.horn_active = true;
+         xstate.autonomous_mode_active = true;
+         lgsvl_state_pub.publish(xstate);
+         x++;
+         std::this_thread::sleep_for(std::chrono::milliseconds(10));
+     }
+}
 
+void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
 
-         xcmd.ctrl_cmd.linear_acceleration =0.1;
+    static int x = 0;
+    iv::brain::decition xdecition;
+    std::cout<<" nSize : "<<nSize<<std::endl;
+    if(!xdecition.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" UpdateDecition Parse error."<<std::endl;
+        return;
+    }
+    /*
+         autoware_msgs::VehicleCmd xcmd;
+          xcmd.twist_cmd.twist.linear.x =   xdecition.speed()/3.6;
+          xcmd.twist_cmd.twist.angular.z = 3.0* xdecition.wheelangle() /600.0; 
          xcmd.ctrl_cmd.linear_velocity = 30.0;
-         xcmd.ctrl_cmd.steering_angle =0.0; 
-        xcmd.gear_cmd.gear = xcmd.gear_cmd.DRIVE;
-         xcmd.lamp_cmd.l =  0;
-         xcmd.lamp_cmd.r = 0;
+         test_cmd_pub.publish(xcmd);
+    */
+   lgsvl_msgs::VehicleStateData xstate;
+   lgsvl_msgs::VehicleControlData xctrl;
+    if(xdecition.leftlamp() == true)
+    {
+        xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_LEFT;
+    }
+    else
+    {
+        if(xdecition.rightlamp() == true)
+        { 
+            xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_RIGHT;
+        }
+        else
+            xstate.blinker_state = lgsvl_msgs::VehicleStateData::BLINKERS_OFF;
+    }
+         xstate.headlight_state = lgsvl_msgs::VehicleStateData::HEADLIGHTS_OFF;
+         xstate.wiper_state = lgsvl_msgs::VehicleStateData::WIPERS_OFF;
+         xstate.current_gear = lgsvl_msgs::VehicleStateData::GEAR_DRIVE;
+         xstate.vehicle_mode = lgsvl_msgs::VehicleStateData::VEHICLE_MODE_COMPLETE_AUTO_DRIVE;
+         xstate.hand_brake_active = false;
+         xstate.horn_active = true;
+         xstate.autonomous_mode_active = true;
+    
+    if(xdecition.brake() < 0.00001)
+    {
+        xctrl.acceleration_pct = xdecition.accelerator() /5.0;
+        if(xctrl.acceleration_pct >1.0)xctrl.acceleration_pct = 1.0;
+        if(xctrl.acceleration_pct<0.0)xctrl.acceleration_pct = 0.0;
+        xctrl.braking_pct = 0.0;
+    }
+    else
+    {
+        xctrl.acceleration_pct = 0.0;
+        xctrl.braking_pct = xdecition.brake()/100.0;
+    }
 
+    //        xctrl.acceleration_pct = 0.05;
+     //   xctrl.braking_pct = 0;
+    xctrl.target_gear =  lgsvl_msgs::VehicleControlData::GEAR_DRIVE;
+    xctrl.target_wheel_angle = 0.1 *  (-1)* xdecition.wheelangle() *M_PI/180.0;
+    xctrl.target_wheel_angular_rate = 300 * M_PI/180.0;
+
+    lgsvl_state_pub.publish(xstate);
+    lgsvl_cmd_pub.publish(xctrl);
 
-         test_cmd_pub.publish(xcmd);
-         x++;
-         std::this_thread::sleep_for(std::chrono::milliseconds(10));
-     }
 }
 
 int main(int argc, char *argv[])
@@ -221,19 +292,22 @@ int main(int argc, char *argv[])
 
     test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
                 "/vehicle_cmd", 10);
+    lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state",10);
+    lgsvl_cmd_pub = private_nh.advertise<lgsvl_msgs::VehicleControlData>("/vehicle_ctrl",10);
 
 	gpa_lidar = iv::modulecomm::RegisterSend(_point_msgname.data(),20000000,1);
     gpa_image = iv::modulecomm::RegisterSend(_image_msgname.data(),20000000,1);
     gpa_odom = iv::modulecomm::RegisterSend(_odom_msgname.data(),10000,1);
 
-   points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
+  points_sub = private_nh.subscribe(_point_topic, 1, points_callback);
 
       image_transport::ImageTransport it(private_nh);
     image_transport::Subscriber sub = it.subscribe( _image_topic, 1, imageCalllback );
 
     odom_sub = private_nh.subscribe(_odom_topic,1,odomCalllback);
 
-    std::thread * pnew = new std::thread(testvh);
+   void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
+  //   std::thread * pnew = new std::thread(testvh);
 
    ros::spin();
    /*

src/ui/ui_ads_hmi/ADCIntelligentVehicle.cpp

@@ -629,6 +629,8 @@ void ADCIntelligentVehicle::timeoutslot()
 
     if((mTimeState.elapsed()-mnTimeUpdateGPS) > 1000)
     {
+        oldrtkstatus = -1;
+        oldinstatus = -1;
         ui->pushButton_10->setStyleSheet("background-color: red");
         ui->pushButton_11->setStyleSheet("background-color: red");
         ui->pushButton_23->setStyleSheet("background-color: red");