change deciton_brain. add obs_mobieye for use mobieye calcualte obs.

src/decition/decition_brain/decision/decision.pri

@@ -1,4 +1,5 @@
-HEADERS += \  
+HEADERS += \
+    $$PWD/obs_mobieye.h   \
     $$PWD/decition_type.h \
     $$PWD/decition_maker.h \
     $$PWD/decide_gps_00.h \
@@ -31,6 +32,7 @@ SOURCES += \
     $$PWD/decide_gps_00.cpp \
     $$PWD/brain.cpp \
     $$PWD/decide_line_00_.cpp \
+    $$PWD/obs_mobieye.cpp \
     $$PWD/obs_predict.cpp \
     $$PWD/adc_tools/transfer.cpp \
     $$PWD/adc_tools/gps_distance.cpp \

src/decition/decition_brain/decision/obs_mobieye.cpp

@@ -0,0 +1,122 @@
+#include "obs_mobieye.h"
+
+#include <math.h>
+
+obs_mobieye::obs_mobieye()
+{
+
+}
+
+void obs_mobieye::UpdateMobieyeObs(iv::mobileye::mobileye &pmobieye)
+{
+    mMutex.lock();
+    mmobieye.CopyFrom(pmobieye);
+    mnUpdateTime = QDateTime::currentMSecsSinceEpoch();
+    mMutex.unlock();
+}
+
+void obs_mobieye::InterpolationTrace(std::vector<iv::Point2D> *pgpsTrace, std::vector<iv::Point2D> &xgpstrace)
+{
+    xgpstrace = *pgpsTrace;
+    unsigned int k;
+    for(k=0;k<(xgpstrace.size()-1);k++)
+    {
+        double fdis;
+        fdis = sqrt(pow(xgpstrace[k].x - xgpstrace[k+1].x,2)
+                    +pow(xgpstrace[k].y - xgpstrace[k+1].y,2));
+        if(fdis > 0.3)
+        {
+            qDebug("fdis %f is big, insert a point2d ",fdis);
+            iv::Point2D xpoint;
+            xpoint.x = (xgpstrace[k].x + xgpstrace[k+1].x)/2.0;
+            xpoint.y = (xgpstrace[k].y + xgpstrace[k+1].y)/2.0;
+
+            xgpstrace.insert((xgpstrace.begin() +k+1),xpoint);
+            if(k>0)k = k-1;
+        }
+    }
+}
+
+int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobileye &xmobieye,
+                                 const double fveh_width,std::vector<iv::mobieyeobstotrace>  & xvectoroptobs)
+{
+    unsigned int i;
+    unsigned int nobjsize = xmobieye.xobj_size();
+    ;
+    for(i=0;i<nobjsize;i++)
+    {
+        iv::mobileye::obs * pobs = xmobieye.mutable_xobj(i);
+
+
+        unsigned int j;
+        unsigned int ntracecount = xgpstrace.size();
+        double fdismin = 1000.0;
+        int index = -1;
+        for(j=0;j<ntracecount;j++)
+        {
+            if(fabs(pobs->pos_x() - xgpstrace[j].y) > 10.0)
+            {
+                continue;
+            }
+            if(fabs(pobs->pos_x() - xgpstrace[j].x) > 10.0)
+            {
+                continue;
+            }
+            double fdis = sqrt(pow(xgpstrace[j].x - pobs->pos_y(),2)
+                               +pow(xgpstrace[j].y - pobs->pos_x(),2));
+            if(fdis<fdismin)
+            {
+                fdismin = fdis;
+                index = j;
+            }
+
+        }
+
+        if(fdismin < (fveh_width + pobs->obswidth()/2.0))
+        {
+            iv::mobieyeobstotrace xmt;
+            xmt.obsindex = i;
+            xmt.traceindex = index;
+            xvectoroptobs.push_back(xmt);
+        }
+    }
+
+}
+
+int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
+                                   double &xobsdis, double &xobsspeed, double &xttc, double &xobsid,const double fveh_width)
+{
+    xobsdis = -1;
+    xobsspeed = 0;
+    xttc = 300;
+    xobsid = -1;
+    if(pgpsTrace->size()<1)return -1;
+    iv::mobileye::mobileye xmobieye;
+    if((QDateTime::currentMSecsSinceEpoch() - mnUpdateTime) > 3000)
+    {
+        qDebug("more than 3 secons no mobieye data.");
+        return -2;
+    }
+
+    mMutex.lock();
+    xmobieye.CopyFrom(mmobieye);
+    mMutex.unlock();
+
+
+    std::vector<iv::Point2D> xgpstrace;
+
+    InterpolationTrace(pgpsTrace,xgpstrace);
+
+
+    std::vector<iv::mobieyeobstotrace> xvectoroptobs;
+
+    GetCandidataObs(xgpstrace,xmobieye,fveh_width,xvectoroptobs);
+
+    if(xvectoroptobs.size() < 1)
+    {
+
+    }
+
+    return 0;
+
+}

src/decition/decition_brain/decision/obs_mobieye.h

@@ -0,0 +1,50 @@
+#ifndef OBS_MOBIEYE_H
+#define OBS_MOBIEYE_H
+
+#include <QMutex>
+
+#include "mobileye.pb.h"
+
+#include "modulecomm.h"
+
+#include <common/gps_type.h>
+
+
+namespace  iv {
+struct  mobieyeobstotrace
+{
+public:
+    int traceindex;
+    int obsindex;
+};
+
+}
+
+class obs_mobieye
+{
+public:
+    obs_mobieye();
+
+private:
+    void * mpa;
+
+    iv::mobileye::mobileye mmobieye;
+    qint64 mnUpdateTime;
+    QMutex mMutex;
+
+public:
+    void UpdateMobieyeObs(iv::mobileye::mobileye & pmobieye);
+public:
+    int GetObsFromMobieye(std::vector<iv::Point2D> * pgpsTrace,double & xobsdis, double & xobsspeed, double & xttc,
+                          double & xobsid,const double fveh_width);
+
+
+
+private:
+    void InterpolationTrace(std::vector<iv::Point2D> * pgpsTrace,std::vector<iv::Point2D> & xgpstrace);
+    int GetCandidataObs(std::vector<iv::Point2D> & xgpstrace,iv::mobileye::mobileye & xmobieye,
+                        const double fveh_width,std::vector<iv::mobieyeobstotrace>  & xvectoroptobs);
+
+};
+
+#endif // OBS_MOBIEYE_H

src/decition/decition_brain/decition_brain.pro

@@ -17,8 +17,13 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += $$PWD/../common/main.cpp \
+    ../../include/msgtype/areaarray.pb.cc \
     ../../include/msgtype/brainstate.pb.cc \
     ../../include/msgtype/decition.pb.cc \
+    ../../include/msgtype/mobileye.pb.cc \
+    ../../include/msgtype/mobileye_lane.pb.cc \
+    ../../include/msgtype/mobileye_obs.pb.cc \
+    ../../include/msgtype/mobileye_tsr.pb.cc \
     ../../include/msgtype/radarobjectarray.pb.cc \
     ../../include/msgtype/radarobject.pb.cc \
     ../../include/msgtype/gpsimu.pb.cc \
@@ -61,6 +66,10 @@ unix:!macx: DEPENDPATH += $$PWD/.
 HEADERS += \
     ../../include/msgtype/brainstate.pb.h \
     ../../include/msgtype/decition.pb.h \
+    ../../include/msgtype/mobileye.pb.h \
+    ../../include/msgtype/mobileye_lane.pb.h \
+    ../../include/msgtype/mobileye_obs.pb.h \
+    ../../include/msgtype/mobileye_tsr.pb.h \
     ../../include/msgtype/radarobjectarray.pb.h \
     ../../include/msgtype/radarobject.pb.h \
     ../../include/msgtype/gpsimu.pb.h \