change driver_map_xodrload, advise mapdata.proto data use proto map data. Add driver_odomtogpsimu for conver gps data to odom.

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/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)
 {
@@ -107,8 +113,8 @@ void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int
     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 = -13.0;
-    double fix_y = 4.0;
+    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);
@@ -167,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;
 }
@@ -175,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/pilottoros/CMakeLists.txt

@@ -121,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)
 {
@@ -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();