Merge branch 'master' of http://47.96.250.93:3000/adc_pilot/modularization

src/ui/ui_ads_hmi/ADCIntelligentVehicle.cpp

@@ -291,6 +291,14 @@ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
     ModuleFun funplantrace = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpaplantrace = iv::modulecomm::RegisterRecvPlus("plantrace",funplantrace);
 
+    ModuleFun funplantrace_left = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_left,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpaplantrace_left = iv::modulecomm::RegisterRecvPlus("obstraceleft",funplantrace_left);
+
+    ModuleFun funplantrace_right = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_right,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpaplantrace_right = iv::modulecomm::RegisterRecvPlus("obstraceright",funplantrace_right);
+
+
+
     ModuleFun funmap = std::bind(&ADCIntelligentVehicle::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpa = iv::modulecomm::RegisterRecvPlus("tracemap",funmap);
     //   mpa = iv::modulecomm::RegisterRecv("tracemap",ListenTraceMap);
@@ -879,11 +887,18 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
         navigation_data = m_navigation_data;
         mMutexNavi.unlock();
 
-        std::vector<iv::Point2D> myplan;
+        std::vector<iv::Point2D> myplan,myplan_left,myplan_right;
         mMutexPlan.lock();
         myplan = m_plan;
         mMutexPlan.unlock();
 
+        mMutexPlan_left.lock();
+        myplan_left = m_plan_left;
+        mMutexPlan_left.unlock();
+
+        mMutexPlan_right.lock();
+        myplan_right = m_plan_right;
+        mMutexPlan_right.unlock();
 //        std::cout<<"plan size is "<<myplan.size()<<std::endl;
 
 
@@ -995,10 +1010,17 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
             y0[i] = xx * sin(lng[0] * PI / 180) + yy * cos(lng[0] * PI / 180);
             k1 = sin((90 + (lng[i] - lng[0])) * PI / 180);
             k2 = cos((90 + (lng[i] - lng[0])) * PI / 180);
+            #if 0
             x1[i] = x0[i] + k1 * 5;
             y1[i] = y0[i] + k2 * 5;
             x2[i] = x0[i] - k1 * 5;
             y2[i] = y0[i] - k2 * 5;
+            #else
+            x1[i] = x0[i] + k1 * 1.75;
+            y1[i] = y0[i] + k2 * 1.75;
+            x2[i] = x0[i] - k1 * 1.75;
+            y2[i] = y0[i] - k2 * 1.75;
+            #endif
         }
 
         double kx_small = (double)(600) / (abs(y_max - y_min));//x轴的系数
@@ -1042,13 +1064,45 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
         QPointF tracePoints[myplan.size()];
         if(myplan.size()>2)
         {
-        for(int i=0;i<(myplan.size()-1);i++){
-            tracePoints[i].setX((float)(450+myplan[i].x*kx));
-            tracePoints[i].setY((float)(700-myplan[i].y*ky));
-            painter->drawLine((float)(450+myplan[i].x*kx),(float)(700-myplan[i].y*ky),
-                              (float)(450+myplan[i+1].x*kx),(float)(700-myplan[i+1].y*ky));
+            for(int i=0;i<(myplan.size()-1);i++)
+            {
+                tracePoints[i].setX((float)(450+myplan[i].x*kx));
+                tracePoints[i].setY((float)(700-myplan[i].y*ky));
+                painter->drawLine((float)(450+myplan[i].x*kx),(float)(700-myplan[i].y*ky),
+                                  (float)(450+myplan[i+1].x*kx),(float)(700-myplan[i+1].y*ky));
+            }
+        }
+
+        penPoint.setColor(Qt::blue);
+        penPoint.setWidth(2);
+        painter->setPen(penPoint);
+        QPointF tracePoints_left[myplan_left.size()];
+        if(myplan_left.size()>2)
+        {
+            for(int i=0;i<(myplan_left.size()-1);i++)
+            {
+                tracePoints_left[i].setX((float)(450+myplan_left[i].x*kx));
+                tracePoints_left[i].setY((float)(700-myplan_left[i].y*ky));
+                painter->drawLine((float)(450+myplan_left[i].x*kx),(float)(700-myplan_left[i].y*ky),
+                                  (float)(450+myplan_left[i+1].x*kx),(float)(700-myplan_left[i+1].y*ky));
+            }
         }
+
+        penPoint.setColor(Qt::blue);
+        penPoint.setWidth(2);
+        painter->setPen(penPoint);
+        QPointF tracePoints_right[myplan_right.size()];
+        if(myplan_right.size()>2)
+        {
+            for(int i=0;i<(myplan_right.size()-1);i++)
+            {
+                tracePoints_right[i].setX((float)(450+myplan_right[i].x*kx));
+                tracePoints_right[i].setY((float)(700-myplan_right[i].y*ky));
+                painter->drawLine((float)(450+myplan_right[i].x*kx),(float)(700-myplan_right[i].y*ky),
+                                  (float)(450+myplan_right[i+1].x*kx),(float)(700-myplan_right[i+1].y*ky));
+            }
         }
+
  //       painter->drawPolyline(tracePoints,myplan.size());
         // draw plan trace end
 /////////////////////////////////////apollo add car icon  20200409
@@ -1339,6 +1393,37 @@ void ADCIntelligentVehicle::UpdatePlanTrace(const char * strdata,const unsigned
     mMutexPlan.unlock();
 }
 
+void ADCIntelligentVehicle::UpdatePlanTrace_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    mMutexPlan_left.lock();
+    m_plan_left.clear();
+    int nplansize = nSize/sizeof(iv::Point2D);
+    int npsize = sizeof(iv::Point2D);
+    int i;
+    for(i=0;i<nplansize;i++)
+    {
+        iv::Point2D x;
+        memcpy(&x,strdata + i*npsize,npsize);
+        m_plan_left.push_back(x);
+    }
+    mMutexPlan_left.unlock();
+}
+
+void ADCIntelligentVehicle::UpdatePlanTrace_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    mMutexPlan_right.lock();
+    m_plan_right.clear();
+    int nplansize = nSize/sizeof(iv::Point2D);
+    int npsize = sizeof(iv::Point2D);
+    int i;
+    for(i=0;i<nplansize;i++)
+    {
+        iv::Point2D x;
+        memcpy(&x,strdata + i*npsize,npsize);
+        m_plan_right.push_back(x);
+    }
+    mMutexPlan_right.unlock();
+}
 
 
 /**

src/ui/ui_ads_hmi/ADCIntelligentVehicle.h

@@ -316,10 +316,14 @@ private:
 /***********20200509****************/
 public:
     void UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
-    std::vector<iv::Point2D> m_plan;
-    QMutex mMutexPlan;
+    void UpdatePlanTrace_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void UpdatePlanTrace_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    std::vector<iv::Point2D> m_plan,m_plan_left,m_plan_right;
+    QMutex mMutexPlan,mMutexPlan_left,mMutexPlan_right;
 private:
     void * mpaplantrace;
+    void * mpaplantrace_left;
+    void * mpaplantrace_right;
 /**************************/
 private:
     void * mpaLidar;