change decition_brain. add obs_mobieye for use mobieye to calculate obs distance and ttc.but not complete.

src/decition/decition_brain/decision/decide_gps_00.cpp

@@ -2662,6 +2662,11 @@ void iv::decision::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
     getEsrObsDistance(gpsTrace, roadNum);
 
+    double mobieye_ttc,mobieye_obsid,mobieye_speed,mobieye_distance;
+
+    double fveh_width = 2.0;
+//    MobieyeInst.GetObsFromMobieye(&gpsTrace,mobieye_distance,mobieye_speed,mobieye_ttc,mobieye_obsid,fveh_width);
+
     //   qDebug("time 2 is %d ",xTime.elapsed());
     //  esrDistance=-1;
 

src/decition/decition_brain/decision/decide_gps_00.h

@@ -21,6 +21,8 @@
 #include "ivlog.h"
 #include <memory>
 
+#include "obs_mobieye.h"
+
 namespace iv {
 namespace decision {
 //根据传感器传输来的信息作出决策

src/decition/decition_brain/decision/obs_mobieye.cpp

@@ -2,9 +2,30 @@
 
 #include <math.h>
 
+obs_mobieye * gom;
+
+static void ListenMobieye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)&index;
+    (void)dt;
+    (void)strmemname;
+    iv::mobileye::mobileye xmobieye;
+    if(xmobieye.ParseFromArray(strdata,nSize))
+    {
+        gom->UpdateMobieyeObs(xmobieye);
+    }
+}
+
 obs_mobieye::obs_mobieye()
 {
+    gom = this;
+    mpa = iv::modulecomm::RegisterRecv("mobieye",ListenMobieye);
+}
 
+obs_mobieye & obs_mobieye::GetInst()
+{
+    static obs_mobieye x;
+    return x;
 }
 
 void obs_mobieye::UpdateMobieyeObs(iv::mobileye::mobileye &pmobieye)
@@ -85,18 +106,199 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
 
 
 
+bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
+{
+    double x,y;
+    x = xpoint.x;
+    y = xpoint.y;
+    double rel_x,rel_y;
+    rel_x = x - pobs->pos_y();
+    rel_y = y - pobs->pos_x();
+    double beta = pobs->obsang() *M_PI/180.0;
+    double xrot,yrot;
+    xrot = rel_x *cos(beta) - rel_y*sin(beta);
+    yrot = rel_x *sin(beta) + rel_y*cos(beta);
+
+    if(fabs(xrot)<=(pobs->obswidth()/2.0))
+    {
+        return true;
+    }
+
+    if(fabs(yrot)<=(pobs->obslen()/2.0))
+    {
+        return true;
+    }
+
+    return false;
+}
+
+void obs_mobieye::GetVehiclePoint(std::vector<iv::Point2D> &xgpstrace, int index,
+                                  std::vector<iv::Point2D> &xvectorPoint, const double fveh_width)
+{
+    if(index<0)return;
+    if(index>=xgpstrace.size())
+    {
+        return;
+    }
+
+    double hdg = 0;
+    if(index<(xgpstrace.size()-1))
+    {
+        hdg = CalcHdg(QPointF(xgpstrace[index].x,xgpstrace[index].y),
+                      QPointF(xgpstrace[index+1].x,xgpstrace[index+1].y));
+    }
+    else
+    {
+        hdg = CalcHdg(QPointF(xgpstrace[index-1].x,xgpstrace[index-1].y),
+                      QPointF(xgpstrace[index].x,xgpstrace[index].y));
+    }
+
+    hdg = hdg - M_PI/2.0;
+
+
+    const double fdiv = 0.1;
+    int ncount = fveh_width/fdiv;
+    int i;
+    double x0,y0;
+    x0 = xgpstrace[index].x;
+    y0 = xgpstrace[index].y;
+    for(i=0;i<ncount;i++)
+    {
+        double xtem,ytem;
+        double xp,yp;
+        xtem = (i-ncount/2) * fdiv;
+        ytem = 0;
+        xp = xtem*cos(hdg) - ytem*sin(hdg) + x0;
+        yp = xtem*sin(hdg) + ytem*cos(hdg) + y0;
+        iv::Point2D pointx;
+        pointx.x = xp;
+        pointx.y = yp;
+        xvectorPoint.push_back(pointx);
+    }
+}
+
+double obs_mobieye::CalcHdg(QPointF p0, QPointF p1)
+{
+    double x0,y0,x1,y1;
+    x0 = p0.x();
+    y0 = p0.y();
+    x1 = p1.x();
+    y1 = p1.y();
+    if(x0 == x1)
+    {
+        if(y0 < y1)
+        {
+            return M_PI/2.0;
+        }
+        else
+            return M_PI*3.0/2.0;
+    }
+
+    double ratio = (y1-y0)/(x1-x0);
+
+    double hdg = atan(ratio);
+
+    if(ratio > 0)
+    {
+        if(y1 > y0)
+        {
+
+        }
+        else
+        {
+            hdg = hdg + M_PI;
+        }
+    }
+    else
+    {
+        if(y1 > y0)
+        {
+            hdg = hdg + M_PI;
+        }
+        else
+        {
+            hdg = hdg + 2.0*M_PI;
+        }
+    }
+
+    return hdg;
+}
+
+double obs_mobieye::GetTraceDis(std::vector<iv::Point2D> &xgpstrace, int index)
+{
+    double fdis = 0;
+    int i;
+    for(i=1;i<=index;i++)
+    {
+        fdis = sqrt(pow(xgpstrace[i-1].x-xgpstrace[i].x,2)
+                    +pow(xgpstrace[i-1].y - xgpstrace[i].y,2));
+    }
+    return fdis;
+}
+
 int obs_mobieye::GetObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobileye &xmobieye,
                         const double fveh_width, std::vector<iv::mobieyeobstotrace> &xvectoroptobs,
                         double &xobsdis, double &xobsspeed, double &xttc, double &xobsid)
 {
-    std::vector<QPointF> xPointTrace;
-    std::vector<QPointF> xPointObs;
+//    std::vector<QPointF> xPointTrace;
+//    std::vector<QPointF> xPointObs;
     unsigned int i;
+
+
+    std::vector<iv::candidateobs> xvectorcand;
     for(i=0;i<xvectoroptobs.size();i++)
     {
-        iv::Point2D xPoint = xgpstrace[xvectoroptobs[i].traceindex];
-        iv::mobileye::obs * pobs = xmobieye.xobj(xvectoroptobs[i].obsindex);
+        std::vector<iv::Point2D> xvectorPoint;
+        xvectorPoint.clear();
+        GetVehiclePoint(xgpstrace,xvectoroptobs[i].traceindex,xvectorPoint,fveh_width);
+        bool bInObs = false;
+        unsigned int j;
+        iv::mobileye::obs * pobs = xmobieye.mutable_xobj(xvectoroptobs[i].obsindex);
+        for(j=0;j<xvectorPoint.size();j++)
+        {
+            if(PointInObs(pobs,xvectorPoint[j]))
+            {
+                bInObs = true;
+                break;
+            }
+        }
+        if(bInObs)
+        {
+            iv::candidateobs x;
+            x.xobsdis = GetTraceDis(xgpstrace,xvectoroptobs[i].traceindex);
+            x.xttc = 15;
+            if(x.xobsdis>0)x.xttc = x.xobsdis/pobs->obs_rel_vel_x();
+            x.xobsspeed = pobs->obs_rel_vel_x();
+            x.xobsid = pobs->id();
+            xvectorcand.push_back(x);
+        }
     }
+
+    for(i=0;i<xvectorcand.size();i++)
+    {
+        if((xvectorcand[i].xttc>0)&&(xvectorcand[i].xttc<= 15))
+        {
+            if((xttc>15)||(xttc<0))
+            {
+                xttc = xvectorcand[i].xttc;
+                xobsspeed = xvectorcand[i].xobsspeed;
+                xobsdis = xvectorcand[i].xobsdis;
+                xobsid = xvectorcand[i].xobsid;
+            }
+            else
+            {
+                if(xttc>xvectorcand[i].xttc)
+                {
+                    xttc = xvectorcand[i].xttc;
+                    xobsspeed = xvectorcand[i].xobsspeed;
+                    xobsdis = xvectorcand[i].xobsdis;
+                    xobsid = xvectorcand[i].xobsid;
+                }
+            }
+        }
+    }
+
+    return 0;
 }
 
 int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
@@ -104,7 +306,7 @@ int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
 {
     xobsdis = -1;
     xobsspeed = 0;
-    xttc = 300;
+    xttc = -1;
     xobsid = -1;
     if(pgpsTrace->size()<1)return -1;
     iv::mobileye::mobileye xmobieye;
@@ -133,7 +335,7 @@ int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
         return 0;
     }
 
-
+    GetObs(xgpstrace,xmobieye,fveh_width,xvectoroptobs,xobsdis,xobsspeed,xttc,xobsid);
 
     return 0;
 

src/decition/decition_brain/decision/obs_mobieye.h

@@ -21,6 +21,17 @@ public:
 
 }
 
+namespace iv {
+struct candidateobs
+{
+    double xobsdis;
+    double xobsspeed;
+    double xttc;
+    double xobsid;
+};
+
+}
+
 class obs_mobieye
 {
 public:
@@ -35,11 +46,14 @@ private:
 
 public:
     void UpdateMobieyeObs(iv::mobileye::mobileye & pmobieye);
+    static obs_mobieye & GetInst();
 public:
     int GetObsFromMobieye(std::vector<iv::Point2D> * pgpsTrace,double & xobsdis, double & xobsspeed, double & xttc,
                           double & xobsid,const double fveh_width);
 
 
+public:
+    static double CalcHdg(QPointF p0, QPointF p1);
 
 private:
     void InterpolationTrace(std::vector<iv::Point2D> * pgpsTrace,std::vector<iv::Point2D> & xgpstrace);
@@ -51,8 +65,15 @@ private:
                double & xobsdis, double & xobsspeed, double & xttc,
                                          double & xobsid);
 
-    void GetRectPoint(std::vector<iv::Point2D> & xgpstrace,int index,std::vector<QPointF> & xPoint,const double fveh_width);
-    void GetRectPoint(iv::mobileye::obs * pobs,std::vector<QPointF> & xPoint);
+    bool PointInObs(iv::mobileye::obs * pobs, iv::Point2D xpoint);
+
+    void GetVehiclePoint(std::vector<iv::Point2D> & xgpstrace,int index,std::vector<iv::Point2D> & xvectorPoint,
+                         const double fveh_width);
+
+    double GetTraceDis(std::vector<iv::Point2D> & xgpstrace,int index);
+
 };
 
+#define MobieyeInst obs_mobieye::GetInst()
+
 #endif // OBS_MOBIEYE_H