add driver_odomtogpsimu for convert ros odom message to gpsimu.

include/common.pri

@@ -25,6 +25,7 @@ CONFIG += c++11
 QMAKE_LFLAGS += -no-pie
 
 INCLUDEPATH += $$PWD/../src/include/msgtype
+INCLUDEPATH += $$PWD/../src/common/common
 
 if(contains(DEFINES,MODULECOMM_NO_FASTRTPS)){
 DEFINES += NOT_USEFASTRTPS

include/modulecomm.h

@@ -45,14 +45,14 @@ enum ModuleComm_TYPE
     ModuleComm_UNDEFINE = 4
 };
 
-void * MODULECOMMSHARED_EXPORT RegisterSend(const char * strcommname);
+void *  RegisterSend(const char * strcommname);
 
 
-void * MODULECOMMSHARED_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount
+void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount
                                             ,ModuleComm_TYPE xmctype,const unsigned short nport);
-void * MODULECOMMSHARED_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall,
+void *  RegisterRecv(const char * strcommname,SMCallBack pCall,
                                             ModuleComm_TYPE xmctype ,const char * strip,const unsigned short );
-void * MODULECOMMSHARED_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun,
+void *  RegisterRecvPlus(const char * strcommname,ModuleFun xFun,
                                                 ModuleComm_TYPE xmctype,const char * strip,const unsigned short );
 
 
@@ -68,9 +68,9 @@ void MODULECOMMSHARED_EXPORT Unregister(void * pHandle);
 void MODULECOMMSHARED_EXPORT PauseComm(void * pHandle);
 void MODULECOMMSHARED_EXPORT ContintuComm(void * pHandle);
 
-void * MODULECOMMSHARED_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount);
-void * MODULECOMMSHARED_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall);
-void * MODULECOMMSHARED_EXPORT RegisterRecvPlus(const char * strcommname,ModuleFun xFun);
+void *  RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount);
+void *  RegisterRecv(const char * strcommname,SMCallBack pCall);
+void *  RegisterRecvPlus(const char * strcommname,ModuleFun xFun);
 
 
 }

src/common/common/math/gnss_coordinate_convert.cpp

@@ -0,0 +1,153 @@
+#include <math/gnss_coordinate_convert.h>
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+}

src/common/common/math/gnss_coordinate_convert.h

@@ -0,0 +1,27 @@
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/driver/driver_odomtogpsimu/.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_odomtogpsimu/driver_odomtogpsimu.pro

@@ -0,0 +1,39 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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 it uses 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
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!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_odomtogpsimu/main.cpp

@@ -0,0 +1,174 @@
+#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;
+
+
+void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::ros::odom xodom;
+    if(xodom.ParseFromArray(strdata,nSize) == false)
+    {
+        std::cout<<" ListenODOM Fail. "<<std::endl;
+        return;
+    }
+    std::cout<<" velocity : "<<xodom.twist_linear().x()<<std::endl;
+
+    iv::gps::gpsimu xgpsimu;
+    xgpsimu.set_msgtime(xodom.timestamp() );
+    xgpsimu.set_speed(xodom.twist_linear().x());
+
+
+    EulerAngles xAng;
+    Quaternion quat;
+    quat.x = xodom.pose_orientation().x();
+    quat.y = xodom.pose_orientation().y();
+    quat.z = xodom.pose_orientation().z();
+    quat.w = xodom.pose_orientation().w();
+    xAng = ToEulerAngles(quat);
+
+
+    double x_o,y_o;
+    GaussProjCal(glon0,glat0,&x_o,&y_o);
+    double lon,lat;
+    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);
+    GaussProjInvCal(x_o + rel_x,y_o + rel_y,&lon,&lat);
+    double hdg_c = hdg_o + xAng.yaw;
+
+    hdg_c = M_PI/2.0 - hdg_c;
+    double heading = hdg_c * 180.0/M_PI;
+    while(heading < 0)heading = heading + 360;
+    while(heading >360)heading = heading - 360;
+
+    xgpsimu.set_lat(lat);
+    xgpsimu.set_lon(lon);
+    xgpsimu.set_height(xodom.pose_position().z() + gheight0);
+    xgpsimu.set_heading(heading);
+    xgpsimu.set_pitch(xAng.pitch);
+    xgpsimu.set_roll(xAng.roll);
+
+    double vx,vy;
+    vx = fabs(xodom.twist_linear().x() *cos(xAng.yaw)
+            + xodom.twist_linear().y() * sin(xAng.yaw));
+    vy = fabs(xodom.twist_linear().x() *sin(xAng.yaw)
+            + xodom.twist_linear().y() * cos(xAng.yaw));
+    double ve = vx * cos(hdg_o) - vy * sin(hdg_o);
+    double vn = vx * sin(hdg_o) + vy * cos(hdg_o);
+    double vd = xodom.twist_linear().z() * (-1);
+    xgpsimu.set_ve(ve);
+    xgpsimu.set_vn(vn);
+    xgpsimu.set_vd(vd);
+
+    xgpsimu.set_ins_state(4);
+    xgpsimu.set_rtk_state(6);
+    xgpsimu.set_satnum1(1);
+    xgpsimu.set_satnum2(1);
+
+    int nbytesize = xgpsimu.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nbytesize]);
+    if(xgpsimu.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);
+    ghead0 = 360.0;
+    gheight0 = 0.0;
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    GetParam("./driver_odomtogpsimu");
+
+    gpa = iv::modulecomm::RegisterSend("hcp2_gpsimu",10000,1);
+    iv::modulecomm::RegisterRecv("odom_ros",ListenODOM);
+
+
+    return a.exec();
+}

src/include/proto/odom.proto

@@ -0,0 +1,45 @@
+syntax = "proto2";
+
+package iv.ros;
+
+message pose_position_def
+{
+ optional double x = 1;
+ optional double y = 2;
+ optional double z = 3;
+};
+
+message pose_orientation_def
+{
+ optional double x = 1;
+ optional double y = 2;
+ optional double z = 3;
+ optional double w = 4;
+};
+
+message twist_angular_def
+{
+ optional double x = 1;
+ optional double y = 2;
+ optional double z = 3;
+};
+
+
+message twist_linear_def
+{
+ optional double x = 1;
+ optional double y = 2;
+ optional double z = 3;
+};
+
+
+message odom
+{
+ optional int64 timestamp = 1; //ms 
+ optional pose_position_def pose_position = 2;
+ optional pose_orientation_def pose_orientation = 3;
+ optional twist_angular_def twist_angula = 4;
+ optional twist_linear_def twist_linear = 5;
+
+
+};

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

@@ -116,12 +116,13 @@ catkin_package(
 include_directories(
 ##  INCLUDE_DIRS include
   ${CMAKE_CURRENT_BINARY_DIR}/..
+  ${CMAKE_SOURCE_DIR}/../devel/include
   ${catkin_INCLUDE_DIRS} include
   ${Boost_INCLUDE_DIR}
 )
 
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../include/msgtype/rawpic.pb.cc )
+add_executable(${PROJECT_NAME}_node src/main.cpp ./../../../../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/rostopilot/src/main.cpp

@@ -1,6 +1,7 @@
 #include "ros/ros.h"
 
 #include <memory>
+#include <thread>
 
 #include "modulecomm.h"
 
@@ -15,7 +16,12 @@
 #include<sensor_msgs/image_encodings.h>
 #include<image_transport/image_transport.h>
 
+#include <nav_msgs/Odometry.h>
+
+#include  "autoware_msgs/VehicleCmd.h"
+
 #include "rawpic.pb.h"
+#include "odom.pb.h"
 
 static std::string _point_topic = "/points_raw";
 static std::string _point_msgname = "lidarpc_ros";
@@ -23,10 +29,20 @@ static std::string _point_msgname = "lidarpc_ros";
 static std::string _image_topic = "/image_raw";
 static std::string _image_msgname = "pic_ros";
 
+static std::string  _odom_topic = "/odom";
+static std::string _odom_msgname = "odom_ros";
+
 
   ros::Subscriber points_sub;
+  ros::Subscriber odom_sub;
+
   void * gpa_lidar;
   void * gpa_image;
+  void * gpa_odom;
+
+
+ros::Publisher  test_cmd_pub;
+
 
 static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
 {
@@ -64,8 +80,8 @@ void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
 	try{
 
         cv::Mat mat1 = cv_bridge::toCvShare(msg, "bgr8")->image ;
-        cv::imshow( "video", mat1 );
-        cv::waitKey(30);
+     //   cv::imshow( "video", mat1 );
+      //  cv::waitKey(30);
 
         
                     iv::vision::rawpic pic;
@@ -115,8 +131,72 @@ void imageCalllback(const sensor_msgs::ImageConstPtr& msg)
     }
 }
 
+void  odomCalllback(const  nav_msgs::OdometryConstPtr  & msg)
+{
+  //  std::cout<<" position x "<<msg->pose.pose.position.x<<std::endl;
+    qint64 msgtime = msg->header.stamp.toSec();
+    msgtime = msgtime * 1000;
+    iv::ros::pose_position_def xpose_position;
+    iv::ros::pose_orientation_def xpose_orientation;
+    iv::ros::twist_angular_def twist_angular;
+    iv::ros::twist_linear_def twist_linear;
+    xpose_position.set_x(msg->pose.pose.position.x);
+    xpose_position.set_y(msg->pose.pose.position.y);
+    xpose_position.set_z(msg->pose.pose.position.z);
+    xpose_orientation.set_x(msg->pose.pose.orientation.x);
+    xpose_orientation.set_y(msg->pose.pose.orientation.y);
+    xpose_orientation.set_z(msg->pose.pose.orientation.z);
+    xpose_orientation.set_w(msg->pose.pose.orientation.w);
+    twist_angular.set_x(msg->twist.twist.angular.x);
+    twist_angular.set_y(msg->twist.twist.angular.y);
+    twist_angular.set_z(msg->twist.twist.angular.z);
+    twist_linear.set_x(msg->twist.twist.linear.x);
+    twist_linear.set_y(msg->twist.twist.linear.y);
+    twist_linear.set_z(msg->twist.twist.linear.z);
+    iv::ros::odom xodom;
+    xodom.set_timestamp(msgtime);
+    xodom.mutable_pose_position()->CopyFrom(xpose_position);
+    xodom.mutable_pose_orientation()->CopyFrom(xpose_orientation);
+    xodom.mutable_twist_angula()->CopyFrom(twist_angular);
+    xodom.mutable_twist_linear()->CopyFrom(twist_linear);
+ //   std::cout<<" pose x : "<<xodom.pose_position().x()<<std::endl;
+
+     int nSize = xodom.ByteSize();
+    std::shared_ptr<char>  str_ptr = std::shared_ptr<char>(new char[nSize]);
+     bool bser = xodom.SerializeToArray(str_ptr.get(),nSize);
+     if(bser)iv::modulecomm::ModuleSendMsg(gpa_odom,str_ptr.get(),nSize);
+     else
+    {
+         std::cout<<"odomCalllback "<< "serialize error. "<<std::endl;
+    }
+
+}
 
 
+void testvh()
+{
+     int x = 0;
+     while(1)
+     {
+         autoware_msgs::VehicleCmd xcmd;
+
+         xcmd.header.seq = x;
+
+
+         xcmd.ctrl_cmd.linear_acceleration =0.1;
+         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);
+         x++;
+         std::this_thread::sleep_for(std::chrono::milliseconds(10));
+     }
+}
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "rostopilot");
@@ -134,14 +214,26 @@ int main(int argc, char *argv[])
     std::cout<<"_image_topic is "<<_image_topic<<std::endl;
 	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
 
+        private_nh.getParam("odom_node",_odom_topic);
+	private_nh.getParam("odom_msgname",_odom_msgname);
+    std::cout<<"_odom_topic is "<<_odom_topic<<std::endl;
+	std::cout<<"_odom_msgname : "<<_odom_msgname<<std::endl;
+
+    test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
+                "/vehicle_cmd", 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);
 
       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);
 
    ros::spin();
    /*