Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

include/common.pri

@@ -17,6 +17,11 @@ INCLUDEPATH += $$PWD/../include/
 LIBS += -L$$PWD/../bin/ -lxmlparam -lmodulecomm  -livlog -livfault -livexit -livbacktrace -livchart -livservice
 
 
+!exists(./../bin/libmodulecomm.so){
+message("No library file. Now create common library.")
+system(PWD=`pwd`; cd ..;bash autogen_lib.sh;cd $PWD)
+}
+
 #LIBS += -L$$PWD/../bin/ -lmodulecomm_shm -lmodulecomm_fastrtps -lmodulecomm_fastrtps_tcp -lmodulecomm_inter
 
 unix:LIBS += -lboost_system  -lbacktrace -ldl

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.cpp

@@ -49,6 +49,7 @@ double  iv::decition::Compute00::dBocheAngle;
 
 double iv::decition::Compute00::mfWheelAngle[5];
 SideParkType iv::decition::Compute00::mSideParkType;
+VerticalParkType iv::decition::Compute00::mVerticalParkType;
 
 
 std::vector<int> lastEsrIdVector;
@@ -2182,6 +2183,38 @@ double iv::decition::Compute00::getDecideAngleByLanePID(double realSpeed) {
 }
 
 
+double iv::decition::Compute00::bocheComputeNew(GPS_INS nowGps, GPS_INS aimGps) {
+
+    GaussProjCal(aimGps.gps_lng, aimGps.gps_lat, &aimGps.gps_x, &aimGps.gps_y);
+    Point2D pt = Coordinate_Transfer(nowGps.gps_x, nowGps.gps_y, aimGps);
+
+    double hdgaim = (90 - aimGps.ins_heading_angle) *M_PI/180.0;
+    double hdgnow = (90 - nowGps.ins_heading_angle) *M_PI/180.0;
+
+    double hdgrel = hdgnow - hdgaim;
+
+    VerticalParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius * 1.1,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
+    xCalc.CalcPark();
+
+    std::vector<iv::VerticalParkPoint> xvectorverticalparkpoint;
+    std::vector<double> xvectorwheel;
+    std::vector<double> xvectorlen;
+    double fTotalLen = 0;
+    VerticalParkType xtype = xCalc.GetSolution(xvectorverticalparkpoint,xvectorwheel,xvectorlen,fTotalLen);
+
+    if((xtype == VerticalParkType::ThreeStep) && (fTotalLen < ServiceCarStatus.mfParkValidLen))
+    {
+        nearTpoint=Coordinate_UnTransfer(xvectorverticalparkpoint[0].my*(-1), xvectorverticalparkpoint[0].mx, aimGps);
+        farTpoint = Coordinate_UnTransfer(xvectorverticalparkpoint[1].my*(-1), xvectorverticalparkpoint[1].mx, aimGps);
+        for(int i=0;i<3;i++)mfWheelAngle[i] = xvectorwheel[i];
+        mVerticalParkType = VerticalParkType::ThreeStep;
+        return 1;
+    }
+
+
+    return 0;
+
+}
 
 double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
 
@@ -2550,7 +2583,7 @@ int iv::decition::Compute00::bocheDirectComputeNew(GPS_INS nowGps, GPS_INS aimGp
 
     double hdgrel = hdgnow - hdgaim;
 
-    SideParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
+    SideParkCalc xCalc(pt.y,pt.x*(-1),hdgrel,ServiceCarStatus.mfMinRadius ,ServiceCarStatus.mfMaxWheel,45,ServiceCarStatus.mfParkLastStraightDis);
     xCalc.CalcPark();
 
     std::vector<iv::SideParkPoint> xvectorsideparkpoint;

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/compute_00.h

@@ -9,6 +9,7 @@
 #include <decition/decide_gps_00.h>
 
 #include "sideparkcalc.h"
+#include "verticalparkcalc.h"
 
 namespace iv {
     namespace decition {
@@ -36,6 +37,7 @@ namespace iv {
             static double mfWheelAngle[5];
 
             static SideParkType mSideParkType;
+            static VerticalParkType mVerticalParkType;
 
             static	int getNearestPointIndex(GPS_INS rp, const std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
             static	int getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle);
@@ -76,6 +78,8 @@ namespace iv {
 
             static double bocheCompute(GPS_INS nowGps, GPS_INS aimGps);
 
+            static double bocheComputeNew(GPS_INS nowGps, GPS_INS aimGps);
+
             static	double pingfenxian_xielv(double x_1, double y_1, double x_2, double y_2, double angle_1);
 
             static double getQieXianAngle(GPS_INS nowGps, GPS_INS aimGps);

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/verticalparkcalc.cpp

@@ -0,0 +1,114 @@
+#include "verticalparkcalc.h"
+
+VerticalParkCalc::VerticalParkCalc(double x,double y,double hdg,double fRadius,double MaxWheel ,double MaxAngle ,double fLastDirectDis)
+{
+    mfRaidus = fRadius;
+    mfMaxWheel = MaxWheel;
+    mfMaxAngle = MaxAngle * M_PI/180.0;
+    mfLastDirectDis = fLastDirectDis;
+
+    mx = x;
+    my = y;
+    mhdg =  hdg;
+    normalhdg(mhdg);
+
+    mVerticalParkType = VerticalParkType::NoVSolution;
+}
+
+
+void VerticalParkCalc::normalhdg(double & fhdg)
+{
+    while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
+    while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
+}
+
+void VerticalParkCalc::CalcPark()
+{
+    if((my>0)&&(mhdg<0))return;
+    if((my<0)&&(mhdg>0))return;
+
+    double fhdg = fabs(mhdg);
+    double x= mx;
+    double y = fabs(my);
+    double dy1,dy2,dy3;
+    dy3 = 0;
+    dy2 = mfRaidus * (1 - cos(fhdg));
+    dy1 = y - dy2;
+
+
+
+    if(fhdg < (M_PI/6.0))return;
+
+    if(dy2>=y)
+    {
+        return;
+    }
+
+    double dx1,dx2,dx3;
+    if(fabs(fhdg - M_PI/2.0) < 1e-6)
+    {
+        dx1 = 0;
+    }
+    else
+    {
+        dx1 =  dy1/tan(fhdg);
+    }
+    dx2 = mfRaidus * sin(fhdg);
+    dx3 = x - dx1 - dx2;
+    if(dx3 < mfLastDirectDis)
+    {
+        return;
+    }
+
+    double x0,y0,x1,y1;
+    x1 = dx3;
+    y1 = dy3;
+    x0 = x1 + dx2;
+    if(my>0)
+        y0 = y1 + dy2;
+    else
+    {
+        y0 = y1 - dy2;
+    }
+
+    mvectorpoint.push_back(iv::VerticalParkPoint(x0,y0,mhdg));
+    mvectorpoint.push_back(iv::VerticalParkPoint(x1,y1,0.0));
+
+    mvectorWheel.push_back(0);
+    if(my>0)
+    {
+        mvectorWheel.push_back(mfMaxWheel);
+    }
+    else
+    {
+        mvectorWheel.push_back(mfMaxWheel*(-1.0));
+    }
+    mvectorWheel.push_back(0.0);
+
+    double flen1,flen2,flen3;
+    flen1 = sqrt(pow(mx - x0,2)+pow(my - y0,2));
+    flen2 = fhdg * mfRaidus;
+    flen3 = dx3;
+    mvectorlen.push_back(flen1);
+    mvectorlen.push_back(flen2);
+    mvectorlen.push_back(flen3);
+    mfTotalLen = flen1 + flen2 + flen3;
+
+    mVerticalParkType = VerticalParkType::ThreeStep;
+
+
+}
+
+VerticalParkType VerticalParkCalc::GetSolution(std::vector<iv::VerticalParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
+{
+    if(mVerticalParkType == VerticalParkType::NoVSolution)
+    {
+        return mVerticalParkType;
+    }
+
+    xvectorpoint = mvectorpoint;
+    xvectorWheel = mvectorWheel;
+    xvectorlen = mvectorlen;
+    fTotalLen = mfTotalLen;
+    return mVerticalParkType;
+}

src/decition/decition_brain_sf_changan_shenlan/decition/adc_tools/verticalparkcalc.h

@@ -0,0 +1,63 @@
+#ifndef VERTICALPARKCALC_H
+#define VERTICALPARKCALC_H
+
+
+#include <math.h>
+#include <vector>
+
+
+enum VerticalParkType
+{
+    ThreeStep = 1,  //Wh
+    NoVSolution = 2
+};
+
+namespace iv {
+struct VerticalParkPoint
+{
+    VerticalParkPoint(double x,double y,double fhdg)
+    {
+        mx = x;
+        my = y;
+        mhdg = fhdg;
+    }
+    double mx;
+    double my;
+    double mhdg;
+};
+
+}
+
+
+class VerticalParkCalc
+{
+public:
+    VerticalParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle  = 45.0,double fLastDirectDis = 0.3);
+
+private:
+    double mx,my,mhdg;
+
+    bool mbPark = false;
+
+    double mfLastDirectDis = 0.3;
+    double mfRaidus = 5.0;
+    double mfMaxAngle = 45.0 *M_PI/180.0;
+    double mfMaxWheel = 430.0;
+
+    std::vector<iv::VerticalParkPoint> mvectorpoint;
+    std::vector<double> mvectorWheel; // 3 values
+    std::vector<double> mvectorlen; //3 values
+    double mfTotalLen = 0.0;
+
+    VerticalParkType mVerticalParkType;
+
+private:
+    void normalhdg(double & fhdg);
+
+public:
+    void CalcPark();
+    VerticalParkType GetSolution(std::vector<iv::VerticalParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);
+
+};
+
+#endif // VERTICALPARKCALC_H

src/decition/decition_brain_sf_changan_shenlan/decition/decide_gps_00.cpp

@@ -466,7 +466,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 return gps_decition;
             }else
             {
-                if(trumpet()>3000)
+                if(trumpet()>1000)
                 {
                     trumpetFirstCount=true;
                     vehState=dRever;
@@ -482,7 +482,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         if( vehState!=reverseCar &&  vehState!=reversing  &&  vehState!=reverseCircle &&  vehState!=reverseDirect &&  vehState!=reverseArr )
         {
-            int bocheEN=Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+            int bocheEN=Compute00().bocheComputeNew(now_gps_ins, aim_gps_ins);
             if(bocheEN==1)
             {
                 ServiceCarStatus.bocheEnable=1;
@@ -510,7 +510,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }
             else
             {
-                if(trumpet()>3000)
+                if(trumpet()>1000)
                 {
                     trumpetFirstCount=true;
                     vehState=reverseCar;
@@ -557,7 +557,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     // ChuiZhiTingChe
     if (vehState == reverseCar)
     {
-        Compute00().bocheCompute(now_gps_ins, aim_gps_ins);
+        Compute00().bocheComputeNew(now_gps_ins, aim_gps_ins);
         vehState = reversing;
         qiedianCount=true;
     }
@@ -567,8 +567,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
         iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
 
-         double fdistonear = fabs(pt.x - ptnear.x);
-         if(fdistonear<0.5)  //将吉利项目中的停车逻辑移植过来
+        double fdistonear = pt.x - ptnear.x;
+        if(ptnear.x < 0) fdistonear = ptnear.x - pt.x;
+         if(fdistonear<0.1)
 //        if (dis<2.0)//0.5
         {
             vehState = reverseCircle;
@@ -593,7 +594,10 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     {
         double dis = GetDistance(now_gps_ins, Compute00().farTpoint);
         double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
-        if((((angdis<5)||(angdis>355)))&&(dis<3.0))
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+        iv::Point2D ptfar = iv::decition::Coordinate_Transfer(Compute00().farTpoint.gps_x,Compute00().farTpoint.gps_y,aim_gps_ins);
+        double xx = pt.y - ptfar.y;
+        if((((angdis<3)||(angdis>357)))||(xx<(-0.3)))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         {
             vehState = reverseDirect;
@@ -602,14 +606,14 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         }
         else {
-            if (qiedianCount && trumpet()<0)
+            if (qiedianCount && trumpet()<1500)
                 //         if (qiedianCount && trumpet()<1500)
             {
 
                 //                gps_decition->brake = 10;
                 //                gps_decition->torque = 0;
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else
@@ -626,8 +630,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
             }
 
-            controlAng = Compute00().bocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*1.05;
+            controlAng =  Compute00().mfWheelAngle[1] * (-1);// Compute00().bocheAngle*16.5;
+            gps_decition->wheel_angle = 0 - controlAng*1.00;
 
             cout<<"farTpointDistance================"<<dis<<endl;
 
@@ -640,7 +644,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         //	double dis = GetDistance(now_gps_ins, aim_gps_ins);
         Point2D pt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
 
-        if ((pt.y)<0.5)
+        if ((pt.y)<0.1)
         {
 
             qiedianCount=true;
@@ -658,13 +662,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         else {
 
             //if (qiedianCount && trumpet()<0)
-            if (qiedianCount && trumpet()<1000)
+            if (qiedianCount && trumpet()<1500)
             {
 
                 //                gps_decition->brake = 10;
                 //                gps_decition->torque = 0;
                 dSpeed=0;
-                minDecelerate=min(minDecelerate,-0.5f);
+                minDecelerate=min(minDecelerate,-1.5f);
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
             else
@@ -755,7 +759,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         Point2D pt1 = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint1.gps_x,Compute00().dTpoint1.gps_y, aim_gps_ins);
         double xx= (pt1.y-pt2.y);
-        if(xx < 0.1) //(dis<2.0)
+        if(xx < 0.05) //(dis<2.0)
         {
             vehState = dRever2;
             qiedianCount = true;
@@ -763,7 +767,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else
         {
-            controlAng =  Compute00().mfWheelAngle[1];// ServiceCarStatus.mfMaxWheel ;//Compute00().dBocheAngle*16.5;
+            controlAng =  Compute00().mfWheelAngle[1] * (-1);// ServiceCarStatus.mfMaxWheel ;//Compute00().dBocheAngle*16.5;
             gps_decition->wheel_angle = 0 - controlAng;
             if (qiedianCount && trumpet()<1500)
             {
@@ -799,7 +803,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint2.gps_x,Compute00().dTpoint2.gps_y, aim_gps_ins);
         double xx= (pt1.y-pt2.y);
         // if(xx>0)
-        if(xx<0.1)
+        if(xx<0.05)
         {
             GaussProjCal(aim_gps_ins.gps_lng, aim_gps_ins.gps_lat, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
             Point2D ptt = Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y, aim_gps_ins);
@@ -833,7 +837,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                 phaseSpeedDecition(gps_decition, secSpeed, -1, 0, now_gps_ins);
             }
 
-            gps_decition->wheel_angle = Compute00().mfWheelAngle[2] *(-1.0);//0 ;
+            gps_decition->wheel_angle = Compute00().mfWheelAngle[2];//0 ;
             cout<<"farTpointDistance================"<<dis<<endl;
             return gps_decition;
         }
@@ -847,7 +851,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         Point2D pt2 = Coordinate_Transfer(Compute00().dTpoint3.gps_x,Compute00().dTpoint3.gps_y, aim_gps_ins);
         double angdis =abs(now_gps_ins.ins_heading_angle - aim_gps_ins.ins_heading_angle);
         double xx = pt1.y - pt2.y;
-        if((((angdis<2)||(angdis>358))) || (xx < (-0.3)))
+        if((((angdis<3)||(angdis>357))) || (xx < (-0.3)))
             //       if((((angdis<4)||(angdis>356)))&&(dis<2.0))
         // if(xx>0)
   //      if(xx<0.1)
@@ -858,7 +862,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else {
 
-            controlAng =  Compute00().mfWheelAngle[3];//ServiceCarStatus.mfMaxWheel *(-1.0) ;
+            controlAng =  Compute00().mfWheelAngle[3] * (-1);//ServiceCarStatus.mfMaxWheel *(-1.0) ;
             gps_decition->wheel_angle =  0 - controlAng;// 0 - controlAng*0.95;
             if (qiedianCount && trumpet()<1500)
             {

src/decition/decition_brain_sf_changan_shenlan/decition/decition.pri

@@ -1,6 +1,7 @@
 HEADERS += \
     $$PWD/adc_adapter/changan_shenlan_adapter.h \
     $$PWD/adc_tools/sideparkcalc.h \
+    $$PWD/adc_tools/verticalparkcalc.h \
     $$PWD/decition_type.h \
     $$PWD/decition_maker.h \
     $$PWD/decide_gps_00.h \
@@ -37,6 +38,7 @@ HEADERS += \
 SOURCES += \
     $$PWD/adc_adapter/changan_shenlan_adapter.cpp \
     $$PWD/adc_tools/sideparkcalc.cpp \
+    $$PWD/adc_tools/verticalparkcalc.cpp \
     $$PWD/decide_gps_00.cpp \
     $$PWD/brain.cpp \
     $$PWD/decide_line_00_.cpp \

src/python/pythondecisiondemo/PyModuleCommModule.py

@@ -0,0 +1,91 @@
+
+import threading  
+import time  
+
+import modulecommpython
+import numpy as np  
+
+
+modulelock = threading.Lock()  
+nThread = 0
+  
+class PyModuleComm:  
+    def __init__(self,strname):  
+        # 初始化代码...  
+        print("name: ",strname)
+        self.strmemname = strname
+        self.mbRegister = False
+        global nThread
+        nThread = nThread+1
+        self.mnMode = 0
+        print("nThread = ",nThread)
+        self.obj = modulecommpython.get_ca_object()
+        pass  
+
+    def RegisterRecv(self,call):
+        if self.mbRegister:
+            print(" Have register, can't register other.")
+            return
+        print("Register: ",self.strmemname)
+        self.mpcall = call
+        self.mbRegister = True
+        self.mbRun = True
+        self.mpthread = threading.Thread(target=self.threadrecvdata, args=(self.strmemname,))  
+        self.mpthread.start()
+        print("complete create thread.")
+        self.mnMode = 1
+        self.obj.RegisterRecv(self.strmemname)
+
+    def RegiseterSend(self,nSize,nPacCount):
+        if self.mbRegister:
+            print(" Have register, can't register other.")
+            return
+        print("Register: ",self.strmemname)
+        self.mnsize = nSize
+        self.mnPacCount = nPacCount
+        self.mbRegister = True
+        self.mnMode = 2
+        self.obj.RegisterSend(self.strmemname,nSize,nPacCount)
+    
+    def SendData(self,arr,nsendsize):      
+        nrealsize = np.zeros(1, dtype=np.int32)  
+        nrtn = self.obj.SendData(arr,nsendsize,nrealsize)
+  
+    def threadrecvdata(self, arg):  
+        # 这个函数将被线程执行  
+ #       print(f"线程开始执行，参数是 {arg}")  
+        nBuffSize = 1000
+        arr = np.zeros(nBuffSize,dtype=np.int8)
+        recvtime = np.zeros(1,dtype=np.int64)
+        nrealsize = np.zeros(1,dtype=np.int32)
+        while 1:
+            nrtn = self.obj.RecvData(arr,nBuffSize,nrealsize,recvtime)
+            if nrtn > 0:
+                self.mpcall(arr,nrtn,recvtime)
+            else:
+                pass
+            if nrtn < 0:
+                nBuffSize = nrealsize[0] * 2
+                arr = np.zeros(nBuffSize,dtype=np.int8)
+            else:
+                time.sleep(0.001)
+            
+        print("threadrecvdata complete.")
+
+    def stop_thread(self):
+        self.mbRun = False
+        self.mpthread.join()
+  
+    def start_thread(self, arg):  
+        # 创建线程对象，target参数指向要在线程中运行的函数  
+        self.mbRun = True
+        self.mpthread = threading.Thread(target=self.my_function, args=(arg,))  
+          
+        # 启动线程  
+        self.mpthread.start()  
+          
+        # 可以在这里添加其他代码，主线程会继续执行  
+        print("主线程继续执行...")  
+  
+        # 如果需要等待线程结束，可以调用 join() 方法  
+        # thread.join()  

src/python/pythondecisiondemo/mapdata_pb2.py

@@ -0,0 +1,377 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: mapdata.proto
+
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import message as _message
+from google.protobuf import reflection as _reflection
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor.FileDescriptor(
+  name='mapdata.proto',
+  package='iv.map',
+  syntax='proto2',
+  serialized_options=None,
+  create_key=_descriptor._internal_create_key,
+  serialized_pb=b'\n\rmapdata.proto\x12\x06iv.map\"\xdd\x06\n\x08mappoint\x12\x12\n\x07gps_lat\x18\x01 \x01(\x01:\x01\x30\x12\x12\n\x07gps_lng\x18\x02 \x01(\x01:\x01\x30\x12\x10\n\x05gps_x\x18\x03 \x01(\x01:\x01\x30\x12\x10\n\x05gps_y\x18\x04 \x01(\x01:\x01\x30\x12\x10\n\x05gps_z\x18\x05 \x01(\x01:\x01\x30\x12\x1c\n\x11ins_heading_angle\x18\x06 \x01(\x01:\x01\x30\x12\x15\n\nspeed_mode\x18\x07 \x01(\x05:\x01\x30\x12\x10\n\x05mode2\x18\x08 \x01(\x05:\x01\x30\x12\x10\n\x05speed\x18\t \x01(\x01:\x01\x30\x12\x13\n\x08roadMode\x18\n \x01(\x05:\x01\x30\x12\x12\n\x07runMode\x18\x0b \x01(\x05:\x01\x30\x12\x12\n\x07roadSum\x18\x0c \x01(\x05:\x01\x31\x12\x12\n\x07roadOri\x18\r \x01(\x05:\x01\x30\x12\x18\n\x0bmfLaneWidth\x18\x0e \x01(\x01:\x03\x33.5\x12\x1c\n\x0fmfDisToLaneLeft\x18\x0f \x01(\x01:\x03\x31.8\x12\x1b\n\x10mnLaneChangeMark\x18\x10 \x01(\x05:\x01\x30\x12\x1c\n\x0fmfDisToRoadLeft\x18\x11 \x01(\x01:\x03\x31.8\x12\x18\n\x0bmfRoadWidth\x18\x12 \x01(\x01:\x03\x33.5\x12\x1b\n\rmbInLaneAvoid\x18\x13 \x01(\x08:\x04true\x12\x19\n\x11gps_lat_avoidleft\x18\x14 \x01(\x01\x12\x19\n\x11gps_lng_avoidleft\x18\x15 \x01(\x01\x12\x1a\n\x12gps_lat_avoidright\x18\x16 \x01(\x01\x12\x1a\n\x12gps_lng_avoidright\x18\x17 \x01(\x01\x12\x17\n\x0fgps_x_avoidleft\x18\x18 \x01(\x01\x12\x17\n\x0fgps_y_avoidleft\x18\x19 \x01(\x01\x12\x18\n\x10gps_x_avoidright\x18\x1a \x01(\x01\x12\x18\n\x10gps_y_avoidright\x18\x1b \x01(\x01\x12\x18\n\tmbnoavoid\x18\x1c \x01(\x08:\x05\x66\x61lse\x12\x16\n\x0bmfCurvature\x18\x1d \x01(\x01:\x01\x30\x12\r\n\x05rel_x\x18\x1e \x01(\x01\x12\r\n\x05rel_y\x18\x1f \x01(\x01\x12\r\n\x05rel_z\x18  \x01(\x01\x12\x0f\n\x07rel_yaw\x18! \x01(\x01\x12\x0e\n\x06laneid\x18\" \x01(\x05\x12\x14\n\x0cleftlanecout\x18# \x01(\x05\x12\x16\n\x0erightlanecount\x18$ \x01(\x05\x12\x0e\n\x06roadid\x18% \x01(\x05\x12\r\n\x05index\x18& \x01(\x05\";\n\x08tracemap\x12\x1f\n\x05point\x18\x01 \x03(\x0b\x32\x10.iv.map.mappoint\x12\x0e\n\x06nState\x18\x02 \x01(\x05'
+)
+
+
+
+
+_MAPPOINT = _descriptor.Descriptor(
+  name='mappoint',
+  full_name='iv.map.mappoint',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  create_key=_descriptor._internal_create_key,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='gps_lat', full_name='iv.map.mappoint.gps_lat', index=0,
+      number=1, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_lng', full_name='iv.map.mappoint.gps_lng', index=1,
+      number=2, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_x', full_name='iv.map.mappoint.gps_x', index=2,
+      number=3, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_y', full_name='iv.map.mappoint.gps_y', index=3,
+      number=4, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_z', full_name='iv.map.mappoint.gps_z', index=4,
+      number=5, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='ins_heading_angle', full_name='iv.map.mappoint.ins_heading_angle', index=5,
+      number=6, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='speed_mode', full_name='iv.map.mappoint.speed_mode', index=6,
+      number=7, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mode2', full_name='iv.map.mappoint.mode2', index=7,
+      number=8, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='speed', full_name='iv.map.mappoint.speed', index=8,
+      number=9, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='roadMode', full_name='iv.map.mappoint.roadMode', index=9,
+      number=10, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='runMode', full_name='iv.map.mappoint.runMode', index=10,
+      number=11, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='roadSum', full_name='iv.map.mappoint.roadSum', index=11,
+      number=12, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=1,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='roadOri', full_name='iv.map.mappoint.roadOri', index=12,
+      number=13, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mfLaneWidth', full_name='iv.map.mappoint.mfLaneWidth', index=13,
+      number=14, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(3.5),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mfDisToLaneLeft', full_name='iv.map.mappoint.mfDisToLaneLeft', index=14,
+      number=15, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(1.8),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mnLaneChangeMark', full_name='iv.map.mappoint.mnLaneChangeMark', index=15,
+      number=16, type=5, cpp_type=1, label=1,
+      has_default_value=True, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mfDisToRoadLeft', full_name='iv.map.mappoint.mfDisToRoadLeft', index=16,
+      number=17, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(1.8),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mfRoadWidth', full_name='iv.map.mappoint.mfRoadWidth', index=17,
+      number=18, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(3.5),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mbInLaneAvoid', full_name='iv.map.mappoint.mbInLaneAvoid', index=18,
+      number=19, type=8, cpp_type=7, label=1,
+      has_default_value=True, default_value=True,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_lat_avoidleft', full_name='iv.map.mappoint.gps_lat_avoidleft', index=19,
+      number=20, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_lng_avoidleft', full_name='iv.map.mappoint.gps_lng_avoidleft', index=20,
+      number=21, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_lat_avoidright', full_name='iv.map.mappoint.gps_lat_avoidright', index=21,
+      number=22, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_lng_avoidright', full_name='iv.map.mappoint.gps_lng_avoidright', index=22,
+      number=23, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_x_avoidleft', full_name='iv.map.mappoint.gps_x_avoidleft', index=23,
+      number=24, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_y_avoidleft', full_name='iv.map.mappoint.gps_y_avoidleft', index=24,
+      number=25, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_x_avoidright', full_name='iv.map.mappoint.gps_x_avoidright', index=25,
+      number=26, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='gps_y_avoidright', full_name='iv.map.mappoint.gps_y_avoidright', index=26,
+      number=27, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mbnoavoid', full_name='iv.map.mappoint.mbnoavoid', index=27,
+      number=28, type=8, cpp_type=7, label=1,
+      has_default_value=True, default_value=False,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='mfCurvature', full_name='iv.map.mappoint.mfCurvature', index=28,
+      number=29, type=1, cpp_type=5, label=1,
+      has_default_value=True, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='rel_x', full_name='iv.map.mappoint.rel_x', index=29,
+      number=30, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='rel_y', full_name='iv.map.mappoint.rel_y', index=30,
+      number=31, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='rel_z', full_name='iv.map.mappoint.rel_z', index=31,
+      number=32, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='rel_yaw', full_name='iv.map.mappoint.rel_yaw', index=32,
+      number=33, type=1, cpp_type=5, label=1,
+      has_default_value=False, default_value=float(0),
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='laneid', full_name='iv.map.mappoint.laneid', index=33,
+      number=34, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='leftlanecout', full_name='iv.map.mappoint.leftlanecout', index=34,
+      number=35, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='rightlanecount', full_name='iv.map.mappoint.rightlanecount', index=35,
+      number=36, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='roadid', full_name='iv.map.mappoint.roadid', index=36,
+      number=37, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='index', full_name='iv.map.mappoint.index', index=37,
+      number=38, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto2',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=26,
+  serialized_end=887,
+)
+
+
+_TRACEMAP = _descriptor.Descriptor(
+  name='tracemap',
+  full_name='iv.map.tracemap',
+  filename=None,
+  file=DESCRIPTOR,
+  containing_type=None,
+  create_key=_descriptor._internal_create_key,
+  fields=[
+    _descriptor.FieldDescriptor(
+      name='point', full_name='iv.map.tracemap.point', index=0,
+      number=1, type=11, cpp_type=10, label=3,
+      has_default_value=False, default_value=[],
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+    _descriptor.FieldDescriptor(
+      name='nState', full_name='iv.map.tracemap.nState', index=1,
+      number=2, type=5, cpp_type=1, label=1,
+      has_default_value=False, default_value=0,
+      message_type=None, enum_type=None, containing_type=None,
+      is_extension=False, extension_scope=None,
+      serialized_options=None, file=DESCRIPTOR,  create_key=_descriptor._internal_create_key),
+  ],
+  extensions=[
+  ],
+  nested_types=[],
+  enum_types=[
+  ],
+  serialized_options=None,
+  is_extendable=False,
+  syntax='proto2',
+  extension_ranges=[],
+  oneofs=[
+  ],
+  serialized_start=889,
+  serialized_end=948,
+)
+
+_TRACEMAP.fields_by_name['point'].message_type = _MAPPOINT
+DESCRIPTOR.message_types_by_name['mappoint'] = _MAPPOINT
+DESCRIPTOR.message_types_by_name['tracemap'] = _TRACEMAP
+_sym_db.RegisterFileDescriptor(DESCRIPTOR)
+
+mappoint = _reflection.GeneratedProtocolMessageType('mappoint', (_message.Message,), {
+  'DESCRIPTOR' : _MAPPOINT,
+  '__module__' : 'mapdata_pb2'
+  # @@protoc_insertion_point(class_scope:iv.map.mappoint)
+  })
+_sym_db.RegisterMessage(mappoint)
+
+tracemap = _reflection.GeneratedProtocolMessageType('tracemap', (_message.Message,), {
+  'DESCRIPTOR' : _TRACEMAP,
+  '__module__' : 'mapdata_pb2'
+  # @@protoc_insertion_point(class_scope:iv.map.tracemap)
+  })
+_sym_db.RegisterMessage(tracemap)
+
+
+# @@protoc_insertion_point(module_scope)

src/python/pythondecisiondemo/pythondecisiondemo.py

@@ -0,0 +1,63 @@
+import modulecommpython
+import numpy as np  
+import time
+
+import gpsimu_pb2
+import mapdata_pb2
+
+from PyModuleCommModule import PyModuleComm
+
+time_gpsimu = 0
+msg_gpsimu = gpsimu_pb2.gpsimu()
+
+time_mapdata = 0
+msg_mapdata = mapdata_pb2.tracemap()
+
+
+def gpsimu_callback(arr : np,nsize,time):  
+    print("Python callback function called from C++!. time: ",time)
+    print("    size: ",nsize)  
+    sub_arr = arr[0:nsize]
+    databytes = sub_arr.tostring()
+    msg = gpsimu_pb2.gpsimu()
+    msg.ParseFromString(databytes)
+    msg_gpsimu = msg
+    time_gpsimu = time
+    print("lon: ",msg.lon)
+
+
+def mapdata_callback(arr : np,nsize,time):   
+    sub_arr = arr[0:nsize]
+    databytes = sub_arr.tostring()
+    msg = mapdata_pb2.tracemap()
+    msg.ParseFromString(databytes)
+    msg_mapdata = msg
+    time_mapdata = time
+    
+
+def MakeDecision(md : PyModuleComm, msg_mapdata : mapdata_pb2, msg_gpsimu : gpsimu_pb2)
+    pass
+
+    
+
+def main():  
+
+    # 初始化一个变量  
+    count = 0  
+
+    mc = PyModuleComm("hcp2_gpsimu")
+    mc.RegisterRecv(gpsimu_callback)
+
+    mmap = PyModuleComm("newtracemap")
+    mmap.RegisterRecv(mapdata_callback)
+ 
+    # 使用while循环，只要count小于10，就继续循环  
+    while count < 10:
+        time.sleep(1.0)
+
+  
+    print("循环结束！")  
+  
+# 调用main函数  
+if __name__ == "__main__":  
+    main()

src/test/testsideparkcalc/mainwindow.cpp

@@ -42,7 +42,7 @@ MainWindow::MainWindow(QWidget *parent)
     QGraphicsLineItem * pliney = new QGraphicsLineItem(RATIO * X_MOVE,0,RATIO * X_MOVE,VIEW_HEIGHT);
     mpscene->addItem(pliney);
 
-    setWindowTitle("Calc Park   (if HDG>0.5236  Vertical Park , else Side Park)");
+    setWindowTitle("Calc Park   (if HDG>0.5236  Vertical Park , Else Side Park)");
 }
 
 MainWindow::~MainWindow()
@@ -55,7 +55,7 @@ void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,doubl
 {
 
 
-    SideParkCalc xPark(x,y,hdg);
+    SideParkCalc xPark(x,y,hdg,fMinRadius,fMaxWheel,45,fStraightDis);
     xPark.CalcPark();
 
     std::vector<iv::SideParkPoint> xvectorsideparkpoint;
@@ -73,7 +73,7 @@ void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,doubl
     {
         char strout[3000];
         char strtemp[1000];
-        snprintf(strout,3000,"SidePark:\nFive Step:\n");
+        snprintf(strout,3000,"SidePark:\nFive Steps:\n");
         snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
         int i = 0;
         int  npoint =4;
@@ -277,7 +277,7 @@ void MainWindow::SidePark(double x,double y ,double hdg, double fMinRadius,doubl
     {
         char strout[3000];
         char strtemp[1000];
-        snprintf(strout,3000,"SidePark:\nTwo Step:\n");
+        snprintf(strout,3000,"SidePark:\nTwo Steps:\n");
         snprintf(strtemp,1000,"Points:\n");strncat(strout,strtemp,3000);
         int i = 0;
         int  npoint =1;

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -70,14 +70,29 @@ LIBS += -lshowxodrinvtk
 INCLUDEPATH += $$PWD/../../common/common/math/
 
 LIBS += -lboost_system
-#LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
-#        -lvtkFiltersSources-6.3
 
-#LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
-#        -lvtkFiltersSources-9.1
 
+system(. /etc/lsb-release;echo $DISTRIB_RELEASE)
+
+
+#infile( /etc/lsb-release, DISTRIB_RELEASE, 22.04 ){
+#message("system is 22.04")
+#}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 18.04 ){
+LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+        -lvtkFiltersSources-6.3
+}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 22.04 ){
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
+}
+
+infile( /etc/lsb-release, DISTRIB_RELEASE, 20.04 ){
 LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
         -lvtkFiltersSources-7.1
+}
 
 FORMS += \
     mainwindow.ui