add driver_autoware_computewaypoints compute waypoints for autoware.

src/driver/driver_autoware_computewaypoints/.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_autoware_computewaypoints/computewaypoints.cpp

@@ -0,0 +1,232 @@
+#include "computewaypoints.h"
+
+#include <limits>
+#include <memory>
+
+ComputeWayPoints::ComputeWayPoints()
+{
+    ModuleFun xFunGPSIMU = std::bind(&ComputeWayPoints::ListenGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",xFunGPSIMU);
+
+    ModuleFun xFunNewTraceMap = std::bind(&ComputeWayPoints::ListenNewTraceMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpanewtrace = iv::modulecomm::RegisterRecvPlus("newtracemap",xFunNewTraceMap);
+
+    mpawaypoints = iv::modulecomm::RegisterSend("waypoints",1000000,1);
+
+    mpthread = new std::thread(&ComputeWayPoints::ThreadCompute,this);
+}
+
+ComputeWayPoints::~ComputeWayPoints()
+{
+    mbCompute = false;
+    mpthread->join();
+}
+
+void ComputeWayPoints::ThreadCompute()
+{
+    while(mbCompute)
+    {
+        if(mbHaveMap == false)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            continue;
+        }
+        if(mbGPSIMUUpdate == false)
+        {
+            mMutexWait.lock();
+            mwc.wait(&mMutexWait,100);
+            mMutexWait.unlock();
+        }
+        if(mbGPSIMUUpdate)
+        {
+            iv::map::tracemap xtracemap;
+            int nrtn;
+            nrtn = FindWayPoints(xtracemap);
+            mbGPSIMUUpdate = false;
+            if(nrtn > 0)
+            {
+                int nbytesize = xtracemap.ByteSize();
+                std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nbytesize]);
+                if(xtracemap.SerializeToArray(str_ptr.get(),nbytesize))
+                {
+                    iv::modulecomm::ModuleSendMsg(mpawaypoints,str_ptr.get(),nbytesize);
+                }
+                else
+                {
+                    std::cout<<" tracemap serialize fail."<<std::endl;
+                }
+            }
+        }
+    }
+
+    std::cout<<" ThreadCompute Complete."<<std::endl;
+}
+
+
+void ComputeWayPoints::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))
+    {
+        std::cout<<" ListenGPSIMU Parse Fail. "<<std::endl;
+        return;
+    }
+
+    mMutexgpsimu.lock();
+    mgpsimu.CopyFrom(xgpsimu);
+    mbGPSIMUUpdate = true;
+    mMutexgpsimu.unlock();
+    mwc.wakeAll();
+}
+
+void ComputeWayPoints::ListenNewTraceMap(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void )index;
+    (void)dt;
+    (void)strmemname;
+
+    const double fspeedlimt = 30.0/3.6;
+    const double fdacc = -0.5;
+
+    iv::map::tracemap xtracemap;
+    if(!xtracemap.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ListenNewTraceMap Parse Fail. "<<std::endl;
+        return;
+    }
+    int nmapsize = xtracemap.point_size();
+    int i;
+    if(nmapsize<100)return;
+    iv::map::mappoint * plastpoint = xtracemap.mutable_point(nmapsize -1);
+    plastpoint->set_speed(0);
+    double fdis = 0;
+    for(i=(nmapsize-2);i>=0;i--)
+    {
+        iv::map::mappoint * ppoint = xtracemap.mutable_point(i);
+        fdis = fdis + sqrt(pow(ppoint->gps_x() - plastpoint->gps_x(),2)+pow(ppoint->gps_y() - plastpoint->gps_y(),2));
+        double fv = sqrt(fabs(2.0* fdacc * fdis ));
+        if(fv > fspeedlimt)fv = fspeedlimt;
+        ppoint->set_speed(fv);
+        plastpoint = ppoint;
+ //       std::cout<<" speed : "<<fv<<std::endl;
+
+    }
+
+    mMutexTraceMap.lock();
+    mtracemap.CopyFrom(xtracemap);
+    mbHaveMap = true;
+    mMutexTraceMap.unlock();
+
+}
+
+int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
+{
+    qint64 ncompstart = std::chrono::system_clock::now().time_since_epoch().count();
+    static int nnearoldindex = -1;
+    iv::gps::gpsimu xgpsimu;
+    mMutexgpsimu.lock();
+    xgpsimu.CopyFrom(mgpsimu);
+    mMutexgpsimu.unlock();
+    mMutexTraceMap.lock();
+    double fdis;
+    double fheaddiff;
+    int nnearindex = -1;
+    double fdismin = std::numeric_limits<double>::max();
+
+    double fnowx,fnowy;
+    GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&fnowx,&fnowy);
+
+    int i;
+    int ntracesize = mtracemap.point_size();
+
+    int nfrom = nnearoldindex - 100;
+    if(nfrom < 0)nfrom = 0;
+    int nto = nnearoldindex + 100;
+    if(nto > ntracesize)nto = ntracesize;
+
+    //First, Find in last near.
+    for(i=nfrom;i<nto;i++)
+    {
+        iv::map::mappoint * ppoint  = mtracemap.mutable_point(i);
+
+        fdis = sqrt(pow(ppoint->gps_x() - fnowx,2)+pow(ppoint->gps_y() - fnowy,2));
+        fheaddiff = (xgpsimu.heading() - ppoint->ins_heading_angle())*M_PI/180.0;
+        while(fheaddiff<(-M_PI))fheaddiff = fheaddiff + 2.0*M_PI;
+        while(fheaddiff > M_PI)fheaddiff = fheaddiff - 2.0*M_PI;
+        //First Find,so use nearest.
+        if(fabs(fheaddiff) < M_PI/9.0)
+        {
+            if(fdis < fdismin)
+            {
+                fdismin = fdis;
+                nnearindex = i;
+            }
+        }
+    }
+
+    //if distance < 1.0, so find
+    if(fdismin<1.0)
+    {
+        std::cout<<"use near successfully."<<std::endl;
+    }
+    else
+    {
+
+        for(i=0;i<ntracesize;i++)
+        {
+            iv::map::mappoint * ppoint  = mtracemap.mutable_point(i);
+
+            fdis = sqrt(pow(ppoint->gps_x() - fnowx,2)+pow(ppoint->gps_y() - fnowy,2));
+            fheaddiff = (xgpsimu.heading() - ppoint->ins_heading_angle())*M_PI/180.0;
+            while(fheaddiff<(-M_PI))fheaddiff = fheaddiff + 2.0*M_PI;
+            while(fheaddiff > M_PI)fheaddiff = fheaddiff - 2.0*M_PI;
+            if(fabs(fheaddiff) < M_PI/2.0)
+            {
+                if(fdis < fdismin)
+                {
+                    fdismin = fdis;
+                    nnearindex = i;
+                }
+            }
+
+        }
+    }
+    if(nnearindex >= 0)
+    {
+        nnearoldindex = nnearindex;
+    }
+
+
+//    std::cout<<" fdismin : "<<fdismin<<std::endl;
+
+    if(fdismin > 10.0)
+    {
+        mMutexTraceMap.unlock();
+        return 0;
+    }
+
+    int j = 0;
+    for(i=nnearindex;i<ntracesize;i++)
+    {
+        iv::map::mappoint * pnew = xtracemap.add_point();
+        pnew->CopyFrom(mtracemap.point(i));
+        j++;
+        if(j>1000)break;
+    }
+
+
+//    std::cout<<" waypoint size : "<<xtracemap.point_size()<<std::endl;
+     mMutexTraceMap.unlock();
+
+    qint64 ncalctime =  std::chrono::system_clock::now().time_since_epoch().count() - ncompstart;
+    double fcalc = ncalctime/(1000000.0);
+    (void)fcalc;
+ //   std::cout<<" calc time : "<<fcalc<<" ms "<<std::endl;
+     return 1;
+
+
+}

src/driver/driver_autoware_computewaypoints/computewaypoints.h

@@ -0,0 +1,52 @@
+#ifndef COMPUTEWAYPOINTS_H
+#define COMPUTEWAYPOINTS_H
+
+#include <QMutex>
+#include <QWaitCondition>
+
+#include <thread>
+#include <iostream>
+
+#include "math/gnss_coordinate_convert.h"
+
+#include "gpsimu.pb.h"
+#include "mapdata.pb.h"
+
+#include "modulecomm.h"
+
+
+class ComputeWayPoints
+{
+public:
+    ComputeWayPoints();
+    ~ComputeWayPoints();
+
+private:
+    iv::gps::gpsimu mgpsimu;
+    QMutex mMutexgpsimu;
+    bool mbGPSIMUUpdate = false;
+
+    iv::map::tracemap  mtracemap;
+    QMutex mMutexTraceMap;
+    bool mbHaveMap = false;
+
+    QMutex mMutexWait;
+    QWaitCondition mwc;
+    bool mbCompute = true;
+
+    void ThreadCompute();
+
+    std::thread * mpthread;
+
+    void ListenGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenNewTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+    int FindWayPoints(iv::map::tracemap & xtracemap);
+
+    void * mpagpsimu;
+    void * mpanewtrace;
+    void * mpawaypoints;
+
+};
+
+#endif // COMPUTEWAYPOINTS_H

src/driver/driver_autoware_computewaypoints/driver_autoware_computewaypoints.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 += main.cpp \
+    ../../common/common/math/gnss_coordinate_convert.cpp \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/mapdata.pb.cc \
+    computewaypoints.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/mapdata.pb.h \
+    computewaypoints.h

src/driver/driver_autoware_computewaypoints/main.cpp

@@ -0,0 +1,13 @@
+#include <QCoreApplication>
+
+#include "computewaypoints.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    ComputeWayPoints xCWP;
+    (void)xCWP;
+
+    return a.exec();
+}

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

@@ -53,6 +53,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   pcl_ros
   pcl_conversions
+  autoware_msgs
 )
 find_package(Boost REQUIRED)
 find_package(OpenCV REQUIRED)
@@ -118,9 +119,11 @@ include_directories(
   ${CMAKE_CURRENT_BINARY_DIR}/..
   ${catkin_INCLUDE_DIRS} include
   ${Boost_INCLUDE_DIR}
+  ${autoware_msgs_INCLUDE_DIRS}
 )
 
 ## Declare a C++ executable
-add_executable(${PROJECT_NAME}_node src/main.cpp src  ./../../../../include/msgtype/rawpic.pb.cc ./../../../../include/msgtype/odom.pb.cc)
+add_executable(${PROJECT_NAME}_node src/main.cpp src  ./../../../../include/msgtype/rawpic.pb.cc ./../../../../include/msgtype/odom.pb.cc 
+  ./../../../../include/msgtype/mapdata.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

@@ -2,6 +2,8 @@
 
 #include "modulecomm.h"
 
+#include <QMutex>
+
 #include <pcl_conversions/pcl_conversions.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
@@ -15,8 +17,13 @@
 
 #include <nav_msgs/Odometry.h>
 
+#include "autoware_msgs/Lane.h"
+
 #include "rawpic.pb.h"
 #include "odom.pb.h"
+#include "mapdata.pb.h"
+
+#include <thread>
 
 
 static std::string _point_topic = "/points_raw";
@@ -33,6 +40,13 @@ image_transport::Publisher  image_pub;
 ros::Publisher odom_pub;
 
 
+static iv::map::tracemap gtracemap;
+static QMutex gMutexTraceMap;
+bool gbHaveMap = false;
+
+static bool gbFindNearRun = true;
+
+
 void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 //	std::cout<<" get msg."<<std::endl;
@@ -137,10 +151,133 @@ void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int
     msg.twist.twist.angular.z = xodom.twist_angula().z();
 
 
+    geometry_msgs::Pose xPose = msg.pose.pose;
+    geometry_msgs::Twist xtwist = msg.twist.twist;
+
+
   odom_pub.publish(msg);
 
 }
 
+
+
+#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
+
+
+void ListenWayPointsMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::map::tracemap xtracemap;
+    if(!xtracemap.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenNewTraceMap Parse Fail. "<<std::endl;
+        return;
+    }
+
+    autoware_msgs::Lane xlane;
+    autoware_msgs::Waypoint xpoint;
+    
+
+    int nmapsize = xtracemap.point_size();
+    int i;
+    for(i=0;i<nmapsize;i++)
+    {
+        iv::map::mappoint * ppoint = xtracemap.mutable_point(i);
+        xpoint.pose.pose.position.x = ppoint->rel_x();
+        xpoint.pose.pose.position.y = ppoint->rel_y();
+        xpoint.pose.pose.position.z = ppoint->rel_z();
+        Quaternion quat;
+        quat = ToQuaternion(ppoint->rel_yaw(),0,0);
+        xpoint.pose.pose.orientation.x = quat.x;
+        xpoint.pose.pose.orientation.y = quat.y;
+        xpoint.pose.pose.orientation.z = quat.z;
+        xpoint.pose.pose.orientation.w = quat.w;
+        
+
+    }
+
+
+
+    gMutexTraceMap.lock();
+    gtracemap.CopyFrom(xtracemap);
+    gbHaveMap = true;
+    gMutexTraceMap.unlock();
+}
+
+void ThreadCalcNearPoint()
+{
+    while(gbFindNearRun)
+    {
+        if(gbHaveMap == false)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            continue;
+        }
+        gMutexTraceMap.lock();
+        int nMapSize = gtracemap.point_size();
+        int i;
+        int nnearindex = -1;
+        for(i=0;i<nMapSize;i++)
+        {
+            double fdis;
+            double fheaddiff;
+        }
+        gMutexTraceMap.unlock();
+    }
+}
+
 int main(int argc, char *argv[])
 {
 	ros::init(argc, argv, "pilottoros");