changing decision_brain.

src1/decision/common/adc_decision_function/ivlocalplan_simple.cpp

@@ -0,0 +1,24 @@
+#include "ivlocalplan_simple.h"
+
+#include <iostream>
+
+ivlocalplan_simple::ivlocalplan_simple()
+{
+
+}
+
+std::vector<iv::Point2D> ivlocalplan_simple::GetLocalPlan(iv::GPS_INS now_gps_ins,int nPathPointIndex ,double fSpeed,std::vector<iv::GPSData> gpsMapLine,
+                                                  iv::LidarGridPtr lidarGridPtr)
+{
+    std::vector<iv::Point2D> xvectorplan;
+    xvectorplan.clear();
+    if(nPathPointIndex<0)
+    {
+        std::cout<<" ivlocalplan_simple::GetLocalPlan pathpoint<0. pathpoint = "<<nPathPointIndex<<std::endl;
+        return xvectorplan;
+    }
+
+
+    return  xvectorplan;
+
+}

src1/decision/common/adc_decision_function/ivlocalplan_simple.h

@@ -0,0 +1,16 @@
+#ifndef IVLOCALPLAN_SIMPLE_H
+#define IVLOCALPLAN_SIMPLE_H
+
+#include "ivif_localplan.h"
+
+class ivlocalplan_simple : public ivif_localplan
+{
+public:
+    ivlocalplan_simple();
+
+public:
+    virtual std::vector<iv::Point2D> GetLocalPlan(iv::GPS_INS now_gps_ins,int nPathPointIndex ,double fSpeed,std::vector<iv::GPSData> gpsMapLine,
+                                                  iv::LidarGridPtr lidarGridPtr);
+};
+
+#endif // IVLOCALPLAN_SIMPLE_H

src1/decision/common/adc_decision_function/ivnearpoint_simple.cpp

@@ -0,0 +1,59 @@
+#include "ivnearpoint_simple.h"
+
+
+#include "adc_tools/compute_00.h"
+#include "math/gnss_coordinate_convert.h"
+
+ivnearpoint_simple::ivnearpoint_simple()
+{
+
+}
+
+ivnearpoint_simple::~ivnearpoint_simple()
+{
+
+}
+
+
+int ivnearpoint_simple::FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
+{
+    iv::GPS_INS now_gps_ins;
+    now_gps_ins.gps_lat = fLat;
+    now_gps_ins.gps_lng = fLon;
+    now_gps_ins.ins_heading_angle = fHeading;
+    GaussProjCal(now_gps_ins.gps_lng,now_gps_ins.gps_lat,&now_gps_ins.gps_x,&now_gps_ins.gps_y);
+    int nindex;
+    int lastIndex,mindis,maxAngle;
+    if(mnLastIndex <0)
+    {
+        nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                       lastIndex,
+                                                       mindis,
+                                                       maxAngle);
+        mnLastIndex = nindex;
+        return nindex;
+    }
+
+    nindex = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                   lastIndex,
+                                                   mindis,
+                                                   maxAngle);
+
+    if(nindex < 0)
+    {
+        nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
+                                                       lastIndex,
+                                                       mindis,
+                                                       maxAngle);
+        mnLastIndex = nindex;
+        return nindex;
+    }
+
+    mnLastIndex = nindex;
+    return nindex;
+}
+
+void ivnearpoint_simple::ResetFind()
+{
+    mnLastIndex = -1;
+}

src1/decision/common/adc_decision_function/ivnearpoint_simple.h

@@ -0,0 +1,20 @@
+#ifndef IVNEARPOINT_SIMPLE_H
+#define IVNEARPOINT_SIMPLE_H
+
+#include "ivif_nearpoint.h"
+
+class ivnearpoint_simple : public ivif_nearpoint
+{
+public:
+    ivnearpoint_simple();
+    ~ivnearpoint_simple();
+
+public:
+    virtual int FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
+    virtual void ResetFind();
+
+private:
+    int mnLastIndex = -1;
+};
+
+#endif // IVNEARPOINT_SIMPLE_H

src1/decision/interface/ivdecision.pri

@@ -19,7 +19,9 @@ HEADERS += \
     $$PWD/../../interface/ivmodule.h \
     $$PWD/../../math/gnss_coordinate_convert.h \
     $$PWD/ivdecision.h \
-    $$PWD/ivif_park.h
+    $$PWD/ivif_park.h \
+    $$PWD/ivif_nearpoint.h \
+    $$PWD/ivif_localplan.h
 
 SOURCES += \
     $$PWD/../../../src/include/msgtype/brainstate.pb.cc \
@@ -42,7 +44,9 @@ SOURCES += \
     $$PWD/../../interface/ivmodule.cpp \
     $$PWD/../../math/gnss_coordinate_convert.cpp \
     $$PWD/ivdecision.cpp \
-    $$PWD/ivif_park.cpp
+    $$PWD/ivif_park.cpp \
+    $$PWD/ivif_nearpoint.cpp \
+    $$PWD/ivif_localplan.cpp
 
 INCLUDEPATH += $$PWD/../interface
 INCLUDEPATH += $$PWD/../../interface

src1/decision/interface/ivif_localplan.cpp

@@ -0,0 +1,51 @@
+#include "ivif_localplan.h"
+
+#include "adc_tools/transfer.h"
+
+ivif_localplan::ivif_localplan()
+{
+
+}
+
+ivif_localplan::~ivif_localplan()
+{
+
+}
+
+std::vector<iv::Point2D> ivif_localplan::GetLocalPlan(iv::GPS_INS now_gps_ins,int nPathPointIndex ,double fSpeed,std::vector<iv::GPSData> gpsMapLine,
+                                                  iv::LidarGridPtr lidarGridPtr)
+{
+    (void)nPathPointIndex;
+    (void)gpsMapLine;
+    (void)lidarGridPtr;
+    std::vector<iv::Point2D> xvectorlocal;
+    xvectorlocal.clear();
+
+    double fPreviewDistance = 100.0;
+
+
+    if(nPathPointIndex<0)
+    {
+        return xvectorlocal;
+    }
+
+    int nsize = static_cast<int>( gpsMapLine.size());
+    int i;
+
+    iv::Point2D ptnow = iv::decition::Coordinate_Transfer(gpsMapLine[static_cast<unsigned int>(nPathPointIndex)]->gps_x,gpsMapLine[static_cast<unsigned int>(nPathPointIndex)]->gps_y, now_gps_ins);
+    xvectorlocal.push_back(ptnow);
+    double s = 0;
+
+    for(i=(nPathPointIndex+1);i<nsize;i++)
+    {
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(gpsMapLine[static_cast<unsigned int>(i)]->gps_x,gpsMapLine[static_cast<unsigned int>(i)]->gps_y, now_gps_ins);
+        s = s + sqrt(pow(pt.y - xvectorlocal[xvectorlocal.size() -1].y,2)+pow(pt.x - xvectorlocal[xvectorlocal.size()-1].x,2));
+        if(s<fPreviewDistance)
+            xvectorlocal.push_back(pt);
+        else
+            break;
+
+    }
+
+    return xvectorlocal;
+}

src1/decision/interface/ivif_localplan.h

@@ -0,0 +1,18 @@
+#ifndef IVIF_LOCALPLAN_H
+#define IVIF_LOCALPLAN_H
+
+#include "gps_type.h"
+#include "obstacle_type.h"
+
+class ivif_localplan
+{
+public:
+    ivif_localplan();
+    ~ivif_localplan();
+
+public:
+    virtual std::vector<iv::Point2D> GetLocalPlan(iv::GPS_INS now_gps_ins,int nPathPointIndex ,double fSpeed,std::vector<iv::GPSData> gpsMapLine,
+                                                  iv::LidarGridPtr lidarGridPtr);
+};
+
+#endif // IVIF_LOCALPLAN_H

src1/decision/interface/ivif_nearpoint.cpp

@@ -0,0 +1,25 @@
+#include "ivif_nearpoint.h"
+
+ivif_nearpoint::ivif_nearpoint()
+{
+
+}
+
+ivif_nearpoint::~ivif_nearpoint()
+{
+
+}
+
+int ivif_nearpoint::FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
+{
+    (void)gpsMapLine;
+    (void)fLon;
+    (void)fLat;
+    (void)fHeading;
+    return -1;
+}
+
+void ivif_nearpoint::ResetFind()
+{
+
+}

src1/decision/interface/ivif_nearpoint.h

@@ -0,0 +1,20 @@
+#ifndef IVIF_NEARPOINT_H
+#define IVIF_NEARPOINT_H
+
+
+#include "gps_type.h"
+#include <vector>
+
+class ivif_nearpoint
+{
+public:
+    ivif_nearpoint();
+    ~ivif_nearpoint();
+
+
+public:
+    virtual int FindNearPoint(std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading);
+    virtual void ResetFind();
+};
+
+#endif // IVIF_NEARPOINT_H