change decition_brain. add obs postion ajust.

src/decition/decition_brain/decision/obs_mobieye.cpp

@@ -19,7 +19,7 @@ static void ListenMobieye(const char * strdata,const unsigned int nSize,const un
 obs_mobieye::obs_mobieye()
 {
     gom = this;
-    mpa = iv::modulecomm::RegisterRecv("mobieye",ListenMobieye);
+    mpa = iv::modulecomm::RegisterRecv("mobileye",ListenMobieye);
 }
 
 obs_mobieye & obs_mobieye::GetInst()
@@ -68,6 +68,9 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
     {
         iv::mobileye::obs * pobs = xmobieye.mutable_xobj(i);
 
+        double fobs_x = pobs->pos_y()*(-1);
+        double fobs_y = pobs->pos_x();
+        ajustobspos(fobs_x,fobs_y,mfangajust);
 
         unsigned int j;
         unsigned int ntracecount = xgpstrace.size();
@@ -75,16 +78,16 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
         int index = -1;
         for(j=0;j<ntracecount;j++)
         {
-            if(fabs((pobs->pos_x()*(-1)) - xgpstrace[j].y) > 10.0)
+            if(fabs(fobs_x - xgpstrace[j].x) > 10.0)
             {
                 continue;
             }
-            if(fabs(pobs->pos_x() - xgpstrace[j].y) > 10.0)
+            if(fabs(fobs_y - xgpstrace[j].y) > 10.0)
             {
                 continue;
             }
-            double fdis = sqrt(pow(xgpstrace[j].x - (pobs->pos_y()*(-1.0)),2)
-                               +pow(xgpstrace[j].y - pobs->pos_x(),2));
+            double fdis = sqrt(pow(xgpstrace[j].x - fobs_x,2)
+                               +pow(xgpstrace[j].y - fobs_y,2));
             if(fdis<fdismin)
             {
                 fdismin = fdis;
@@ -98,6 +101,7 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
             iv::mobieyeobstotrace xmt;
             xmt.obsindex = i;
             xmt.traceindex = index;
+            xmt.fdis = fdismin;
             xvectoroptobs.push_back(xmt);
         }
     }
@@ -105,6 +109,15 @@ int obs_mobieye::GetCandidataObs(std::vector<iv::Point2D> &xgpstrace, iv::mobile
 }
 
 
+void obs_mobieye::ajustobspos(double &fobs_x, double fobs_y, double fang)
+{
+    double xtem = fobs_x;
+    double ytem = fobs_y;
+    fobs_x = xtem *cos(fang) - ytem*sin(fang);
+    fobs_y = xtem *sin(fang) + ytem*cos(fang);
+
+}
+
 
 bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
 {
@@ -112,8 +125,13 @@ bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
     x = xpoint.x;
     y = xpoint.y;
     double rel_x,rel_y;
-    rel_x = x - (pobs->pos_y()*(-1));
-    rel_y = y - pobs->pos_x();
+
+    double fobs_x = pobs->pos_y()*(-1);
+    double fobs_y = pobs->pos_x();
+    ajustobspos(fobs_x,fobs_y,mfangajust);
+
+    rel_x = x - fobs_x;
+    rel_y = y - fobs_y;
     double beta = pobs->obsang() *M_PI/180.0;
     double xrot,yrot;
     xrot = rel_x *cos(beta) - rel_y*sin(beta);
@@ -124,10 +142,10 @@ bool obs_mobieye::PointInObs(iv::mobileye::obs *pobs, iv::Point2D xpoint)
         return true;
     }
 
-    if(fabs(yrot)<=(pobs->obslen()/2.0))
-    {
-        return true;
-    }
+//    if(fabs(yrot)<=(pobs->obslen()/2.0))
+//    {
+//        return true;
+//    }
 
     return false;
 }
@@ -230,7 +248,7 @@ double obs_mobieye::GetTraceDis(std::vector<iv::Point2D> &xgpstrace, int index)
     int i;
     for(i=1;i<=index;i++)
     {
-        fdis = sqrt(pow(xgpstrace[i-1].x-xgpstrace[i].x,2)
+        fdis = fdis + sqrt(pow(xgpstrace[i-1].x-xgpstrace[i].x,2)
                     +pow(xgpstrace[i-1].y - xgpstrace[i].y,2));
     }
     return fdis;
@@ -254,6 +272,10 @@ int obs_mobieye::GetObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobil
         bool bInObs = false;
         unsigned int j;
         iv::mobileye::obs * pobs = xmobieye.mutable_xobj(xvectoroptobs[i].obsindex);
+//        if(xvectoroptobs[i].fdis < fveh_width/2.0)
+//        {
+//            bInObs = true;
+//        }
         for(j=0;j<xvectorPoint.size();j++)
         {
             if(PointInObs(pobs,xvectorPoint[j]))
@@ -267,7 +289,7 @@ int obs_mobieye::GetObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobil
             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();
+            if((x.xobsdis>0)&&(pobs->obs_rel_vel_x()!= 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);
@@ -296,15 +318,18 @@ int obs_mobieye::GetObs(std::vector<iv::Point2D> &xgpstrace, iv::mobileye::mobil
                 }
             }
         }
+
     }
 
+
+
     return 0;
 }
 
 int obs_mobieye::GetObsFromMobieye(std::vector<iv::Point2D> *pgpsTrace,
                                    double &xobsdis, double &xobsspeed, double &xttc, double &xobsid,const double fveh_width)
 {
-    xobsdis = -1;
+    xobsdis = 500;
     xobsspeed = 0;
     xttc = -1;
     xobsid = -1;

src/decition/decition_brain/decision/obs_mobieye.h

@@ -17,6 +17,7 @@ struct  mobieyeobstotrace
 public:
     int traceindex;
     int obsindex;
+    double fdis;
 };
 
 }
@@ -72,6 +73,10 @@ private:
 
     double GetTraceDis(std::vector<iv::Point2D> & xgpstrace,int index);
 
+    double mfangajust = 0.0;
+
+    void ajustobspos(double & fobs_x,double fobs_y,double fang);
+
 };
 
 #define MobieyeInst obs_mobieye::GetInst()