chaning decition_brain_localplan. not complete.

src/decition/decition_brain_localplan/brain_localplan_gpstype.cpp

@@ -2,12 +2,13 @@
 
 #include "ivstdcolorout.h"
 
-brain_localplan_gpstype::brain_localplan_gpstype()
+brain_localplan_gpstype::brain_localplan_gpstype(localplan_base * pplan)
 {
 
+    mpplan = pplan;
 
     ModuleFun funmap =std::bind(&brain_localplan_gpstype::UpadateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpa_map = iv::modulecomm::RegisterRecvPlus("tracemap",funmap);
+    mpa_map = iv::modulecomm::RegisterRecvPlus("mapglobal",funmap);
 
     ModuleFun funfusion =std::bind(&brain_localplan_gpstype::UpadateFusion,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpa_fusion = iv::modulecomm::RegisterRecvPlus("li_ra_fusion",funfusion);
@@ -16,7 +17,7 @@ brain_localplan_gpstype::brain_localplan_gpstype()
     mpa_gps = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungps);
 
     ModuleFun funhmi =std::bind(&brain_localplan_gpstype::UpadateHMI,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpa_hmi = iv::modulecomm::RegisterRecvPlus("tracemap",funhmi);
+    mpa_hmi = iv::modulecomm::RegisterRecvPlus("hmi",funhmi);
 
 
     mbRun = true;
@@ -27,21 +28,32 @@ brain_localplan_gpstype::brain_localplan_gpstype()
 brain_localplan_gpstype::~brain_localplan_gpstype()
 {
 
+
+    mbRun = false;
+    mpthreadplan->join();
     iv::modulecomm::Unregister(mpa_hmi);
     iv::modulecomm::Unregister(mpa_gps);
     iv::modulecomm::Unregister(mpa_fusion);
     iv::modulecomm::Unregister(mpa_map);
 
-    mbRun = false;
-    mpthreadplan->join();
 }
 
 
 void brain_localplan_gpstype::threadplan()
 {
+    std::shared_ptr<iv::map::mapglobal>  pmapglobal_ptr = nullptr;
+    std::shared_ptr<iv::gps::gpsimu> pgpsimu_ptr = nullptr;
+    std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr = nullptr;
+
     int nnogps = 0;
     while(mbRun)
     {
+        int64_t nms_now = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(mbDecisionPause)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            continue;
+        }
         if(mbHaveMap == false)
         {
             static int nnotice = 0;
@@ -54,6 +66,19 @@ void brain_localplan_gpstype::threadplan()
             }
             continue;
         }
+
+        bool bNewMap = false;
+
+        if(mbMapUpdate)
+        {
+            pmapglobal_ptr = std::shared_ptr<iv::map::mapglobal>(new iv::map::mapglobal);
+            mmutexmapglobal.lock();
+            pmapglobal_ptr->CopyFrom(mmapglobal);
+            mmutexmapglobal.unlock();
+            mbMapUpdate = false;
+            bNewMap = true;
+        }
+
         static int nnogpsprint = 0;
         if(mbGPSUpdate == false)
         {
@@ -74,10 +99,39 @@ void brain_localplan_gpstype::threadplan()
             }
             continue;
         }
+        else
+        {
+            pgpsimu_ptr = std::shared_ptr<iv::gps::gpsimu>(new iv::gps::gpsimu);
+            mmutexgpsimu.lock();
+            pgpsimu_ptr->CopyFrom(mgpsimu);
+            mmutexgpsimu.unlock();
+            mbGPSUpdate = false;
+            nnogpsprint = 0;
+            nnogps = 0;
+        }
+
+        if(abs(nms_now - mLastms_fusionupdate) > 1000)
+        {
+            pfusion_ptr = nullptr;
+            ivstdcolorout("No Obstacle Information.",iv::STDCOLOR_BOLDYELLOW);
+        }
+        else
+        {
+            if(mbFusionUpdate)
+            {
+                mmutexfusionarray.lock();
+                pfusion_ptr->CopyFrom(mfusionarray);
+                mmutexfusionarray.unlock();
+                mbFusionUpdate = false;
+            }
+        }
+
+        std::shared_ptr<iv::decision::localplan> plocalplan_ptr = mpplan->GetPlan(pmapglobal_ptr,pgpsimu_ptr,pfusion_ptr,bNewMap);
+
+        //Share local plan
+
 
 
-        nnogpsprint = 0;
-        nnogps = 0;
 
         std::this_thread::sleep_for(std::chrono::milliseconds(10));
     }
@@ -89,6 +143,21 @@ void brain_localplan_gpstype::UpadateMap(const char *strdata, const unsigned int
     (void)index;
     (void)dt;
     (void)strmemname;
+    iv::map::mapglobal xmapglobal;
+    if(xmapglobal.ParseFromArray(strdata,static_cast<int>(nSize) ))
+    {
+        mmutexmapglobal.lock();
+        mmapglobal.CopyFrom(xmapglobal);
+        mmutexmapglobal.unlock();
+        mbHaveMap = true;
+        mbMapUpdate = true;
+    }
+    else
+    {
+        char strout[256];
+        snprintf(strout,256,"brain_localplan_gpstype::UpadateMap   Parse mapglobal fail.");
+        ivstdcolorout(strout,iv::STDCOLOR_BOLDYELLOW);
+    }
 }
 
 void brain_localplan_gpstype::UpadateFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
@@ -97,6 +166,21 @@ void brain_localplan_gpstype::UpadateFusion(const char *strdata, const unsigned
     (void)index;
     (void)dt;
     (void)strmemname;
+    iv::fusion::fusionobjectarray xfusionarray;
+    if(xfusionarray.ParseFromArray(strdata,static_cast<int>(nSize) ))
+    {
+        mmutexfusionarray.lock();
+        mfusionarray.CopyFrom(xfusionarray);
+        mmutexfusionarray.unlock();
+        mbFusionUpdate = true;
+        mLastms_fusionupdate = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+    else
+    {
+        char strout[256];
+        snprintf(strout,256,"brain_localplan_gpstype::UpadateFusion   Parse fusionarray fail.");
+        ivstdcolorout(strout,iv::STDCOLOR_BOLDYELLOW);
+    }
 }
 
 void brain_localplan_gpstype::UpadateGPS(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
@@ -105,6 +189,21 @@ void brain_localplan_gpstype::UpadateGPS(const char *strdata, const unsigned int
     (void)index;
     (void)dt;
     (void)strmemname;
+    iv::gps::gpsimu xgpsimu;
+    if(xgpsimu.ParseFromArray(strdata,static_cast<int>(nSize) ))
+    {
+        mmutexgpsimu.lock();
+        mgpsimu.CopyFrom(xgpsimu);
+        mmutexgpsimu.unlock();
+        mbGPSUpdate = true;
+        mLastms_gpsupdate = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+    else
+    {
+        char strout[256];
+        snprintf(strout,256,"brain_localplan_gpstype::UpadateGPS   Parse gpsimu fail.");
+        ivstdcolorout(strout,iv::STDCOLOR_BOLDYELLOW);
+    }
 }
 
 void brain_localplan_gpstype::UpadateHMI(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
@@ -113,5 +212,18 @@ void brain_localplan_gpstype::UpadateHMI(const char *strdata, const unsigned int
     (void)index;
     (void)dt;
     (void)strmemname;
+
+    iv::hmi::hmimsg xhmimsg;
+    if(xhmimsg.ParseFromArray(strdata,static_cast<int>(nSize)))
+    {
+        if(xhmimsg.mbpause())
+        {
+            mbDecisionPause = true;
+        }
+        else
+        {
+            mbDecisionPause = false;
+        }
+    }
 }
 

src/decition/decition_brain_localplan/brain_localplan_gpstype.h

@@ -2,13 +2,21 @@
 #define BRAIN_LOCALPLAN_GPSTYPE_H
 
 #include <thread>
+#include <mutex>
 
 #include "modulecomm.h"
 
+#include "localplan_base.h"
+
+#include "mapglobal.pb.h"
+#include "gpsimu.pb.h"
+#include "fusionobjectarray.pb.h"
+#include "hmi.pb.h"
+
 class brain_localplan_gpstype
 {
 public:
-    brain_localplan_gpstype();
+    brain_localplan_gpstype(localplan_base * pplan);
     ~brain_localplan_gpstype();
 
 private:
@@ -22,6 +30,19 @@ private:
     bool mbMapUpdate = false;
     bool mbGPSUpdate = false;
     int64_t mLastms_gpsupdate = 0;
+    bool mbFusionUpdate = false;
+    int64_t mLastms_fusionupdate = 0;
+
+    localplan_base * mpplan;
+
+    iv::map::mapglobal mmapglobal;
+    std::mutex mmutexmapglobal;
+    iv::gps::gpsimu mgpsimu;
+    std::mutex mmutexgpsimu;
+    iv::fusion::fusionobjectarray mfusionarray;
+    std::mutex mmutexfusionarray;
+
+    bool mbDecisionPause = false;
 
 private:
     void * mpa_map;

src/decition/decition_brain_localplan/decition_brain_localplan.pro

@@ -19,7 +19,11 @@ SOURCES += main.cpp \
     ../../include/msgtype/fusionobjectarray.pb.cc \
     ../../include/msgtype/hmi.pb.cc \
     brain_localplan_gpstype.cpp \
-    ../../include/msgtype/gpsimu.pb.cc
+    ../../include/msgtype/gpsimu.pb.cc \
+    localplan_base.cpp \
+    localplan_follow.cpp \
+    ../../include/msgtype/localplan.pb.cc \
+    ../../include/msgtype/mapglobal.pb.cc
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -34,6 +38,10 @@ HEADERS += \
     ../../include/msgtype/fusionobjectarray.pb.h \
     ../../include/msgtype/hmi.pb.h \
     brain_localplan_gpstype.h \
-    ../../include/msgtype/gpsimu.pb.h
+    ../../include/msgtype/gpsimu.pb.h \
+    localplan_base.h \
+    localplan_follow.h \
+    ../../include/msgtype/localplan.pb.h \
+    ../../include/msgtype/mapglobal.pb.h
 
 

src/decition/decition_brain_localplan/localplan_base.cpp

@@ -0,0 +1,23 @@
+#include "localplan_base.h"
+
+localplan_base::localplan_base()
+{
+
+}
+
+localplan_base::~localplan_base()
+{
+
+}
+
+std::shared_ptr<iv::decision::localplan> localplan_base::GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                                                                 std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap)
+{
+    (void)pmap_ptr;
+    (void)pgps_ptr;
+    (void)pfusion_ptr;
+    (void)bNewMap;
+    std::shared_ptr<iv::decision::localplan> plan_ptr = std::shared_ptr<iv::decision::localplan>(new iv::decision::localplan);
+
+    return plan_ptr;
+}

src/decition/decition_brain_localplan/localplan_base.h

@@ -0,0 +1,19 @@
+#ifndef LOCALPLAN_BASE_H
+#define LOCALPLAN_BASE_H
+
+#include "localplan.pb.h"
+#include "mapglobal.pb.h"
+#include "gpsimu.pb.h"
+#include "fusionobjectarray.pb.h"
+class localplan_base
+{
+public:
+    localplan_base();
+    virtual ~localplan_base();
+
+public:
+    virtual std::shared_ptr<iv::decision::localplan> GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                                                             std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap);;
+};
+
+#endif // LOCALPLAN_BASE_H

src/decition/decition_brain_localplan/localplan_follow.cpp

@@ -0,0 +1,23 @@
+#include "localplan_follow.h"
+
+localplan_follow::localplan_follow()
+{
+
+}
+
+localplan_follow::~localplan_follow()
+{
+
+}
+
+std::shared_ptr<iv::decision::localplan> localplan_follow::GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                                                                   std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap)
+{
+    (void)pmap_ptr;
+    (void)pgps_ptr;
+    (void)pfusion_ptr;
+    (void)bNewMap;
+    std::shared_ptr<iv::decision::localplan> plan_ptr = std::shared_ptr<iv::decision::localplan>(new iv::decision::localplan);
+
+    return plan_ptr;
+}

src/decition/decition_brain_localplan/localplan_follow.h

@@ -0,0 +1,17 @@
+#ifndef LOCALPLAN_FOLLOW_H
+#define LOCALPLAN_FOLLOW_H
+
+#include "localplan_base.h"
+
+class localplan_follow : public localplan_base
+{
+public:
+    localplan_follow();
+    ~localplan_follow();
+
+public:
+    virtual std::shared_ptr<iv::decision::localplan> GetPlan(std::shared_ptr<iv::map::mapglobal> pmap_ptr,std::shared_ptr<iv::gps::gpsimu> pgps_ptr,
+                                                             std::shared_ptr<iv::fusion::fusionobjectarray> pfusion_ptr,bool bNewMap);
+};
+
+#endif // LOCALPLAN_FOLLOW_H

src/decition/decition_brain_localplan/main.cpp

@@ -1,16 +1,33 @@
 #include <QCoreApplication>
 
+#include <csignal>
+
 #include "brain_localplan_gpstype.h"
+#include "localplan_base.h"
+#include "localplan_follow.h"
+
+static brain_localplan_gpstype * gplocalplan_gpstype;
+static localplan_base * gplocalplan;
 
-static brain_localplan_gpstype * gplocalplan;
+void sig_handler(int sig){
+    if ( sig == SIGINT)
+    {
+        delete gplocalplan_gpstype;
+        delete gplocalplan;
+    }
+};
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    brain_localplan_gpstype * pplocalpan = new brain_localplan_gpstype();
-    (void)pplocalpan;
-    gplocalplan = pplocalpan;
+    localplan_base * pplan = dynamic_cast<localplan_base * >(new localplan_follow()) ;
+    gplocalplan = pplan;
+
+    brain_localplan_gpstype * pplocalpan_gpstype = new brain_localplan_gpstype(pplan);
+    gplocalplan_gpstype = pplocalpan_gpstype;
+
+    signal( SIGINT, sig_handler );
 
     return a.exec();
 }

src/include/proto/localplan.proto

@@ -0,0 +1,19 @@
+syntax = "proto2";
+
+package iv.decision;
+
+
+
+message localpoint
+{
+	required double s	=1; // distance to current point;
+	required double x	=2; 
+	required double y	=3;
+};
+
+message localplan
+{
+	repeated localpoint mlocalpoint	= 1;
+};
+
+

src/include/proto/mapglobal.proto

@@ -0,0 +1,44 @@
+syntax = "proto2";
+
+package iv.map;
+
+message pointglobal
+{
+        required double gps_lat = 1 [default=0.0];//纬度
+        required double gps_lng  = 2 [default=0.0];//经度
+        required double ins_heading_angle  = 3 [default=0];	//航向角
+
+        optional double mfCurvature  = 4 [default=0];
+
+        optional double gps_x  = 5 [default=0];   
+        optional double gps_y  = 6 [default=0];
+        optional double gps_z  = 7 [default=0];
+
+        
+        optional double speed  = 8 [default=0];			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+
+        required double mfLaneWidth  = 9 [default=3.5]; // Current Lane Width
+
+        optional double mfDisToLaneLeft  = 10 [default=1.8]; //Distance to Lane Left
+        optional int32 mnLaneChangeMark  = 11 [default=0]; //1 to Left 0 not change   -1 to right
+        optional double mfDisToRoadLeft  = 12 [default=1.8]; //Distance to Road Left
+        optional double mfRoadWidth  = 13 [default=3.5]; // Road Width
+        
+        optional int32  mlanecur = 15; //当前车道编号 1 靠近中心线的第一车道
+        optional int32  mlanecount = 16; //当前道路测的可行驶车道数
+        repeated double mfVectorLaneWidth = 17; //从左到右车道宽       
+        
+        required int32 mnRoadID = 18;        
+        optional string mstrRoadName = 19;
+	
+
+
+};
+
+
+message mapglobal
+{
+	repeated pointglobal point = 1;
+	optional int32 nState  = 2;  
+};