modify sensor fusion

src/decition/common/perception_sf/impl_lidar.cpp

@@ -37,7 +37,6 @@ void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusio
 {
     mMutex.lock();
     fusion_obs.swap(mfusion_obs_);
-    mMutex.unlock();
 
 
     fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
@@ -66,21 +65,30 @@ void iv::perception::LiDARSensor::updataFusion(std::shared_ptr<iv::fusion::fusio
                 fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
                 fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
                 fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().x();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
                 fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
                 fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
                 fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
                 fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
+
+//                if(xobs.centroid().y() < 50 && xobs.centroid().y() >5 )
+//                {
+//                    std::cout<<"     x       y    vy "<<xobs.centroid().x() <<  "     "
+//                            << xobs.centroid().y()<< "     "<<fusion_ptr_[dx*(iv::gry) + dy].speed_y<< "     "
+//                            <<xobs.vel_relative().y()<<std::endl;
+
+//                }
             }
         }
 
     }
- //   signal_fusion_obstacle_grid->operator()(fusion_ptr_);
 
+ //   signal_fusion_obstacle_grid->operator()(fusion_ptr_);
 
 
 
 
+    mMutex.unlock();
 
     signal_lidar_obstacle_grid->operator()(fusion_ptr_);//触发
 

src/decition/common/perception_sf/sensor_lidar.cpp

@@ -61,11 +61,13 @@ void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned
 
 //    for(int i=0;i<xfusion->obj_size();i++)
 //    {
-//        std::cout<<"obj     id:   "<<xfusion->obj(i).id()<<std::endl;
+//        std::cout<<"x    y      vx    vy   w "<<xfusion->obj(i).centroid().x()<<"          "
+//                <<xfusion->obj(i).centroid().y()<<"         "
+//                <<xfusion->obj(i).vel_relative().x()<<"     "
+//                <<xfusion->obj(i).vel_relative().y()<<"     "
+//                <<xfusion->obj(i).dimensions().x()<<std::endl;
 //    }
 
-
-
     iv::perception::gipl->updataFusion(xfusion);
 
 
@@ -104,14 +106,14 @@ void iv::perception::LiDARSensor::start() {
 //    mpa = new adcmemshare("lidar_obs",20000000,3);
 //    mpa->listenmsg(ListenOBS);
 
-    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
+//    iv::modulecomm::RegisterRecv("lidar_per",ListenPer);
 //    mpb = new adcmemshare("lidar_per",20000000,3);
 //    mpb->listenmsg(ListenPer);
 
 
     // txs
 
-    iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
+//    iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
 
     //txs
 

src/decition/decition_brain_sf/decition/adc_tools/compute_00.cpp

@@ -8,6 +8,9 @@
 #include <iostream>
 #include <fstream>
 #include <control/can.h>
+
+#include "ivlog.h"
+extern iv::Ivlog * givlog;
 using namespace std;
 
 #define PI (3.1415926535897932384626433832795)
@@ -659,6 +662,24 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                     {
                         count++; obsPoint.x = ptx; obsPoint.y = pty;
                     }
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+                        //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+                        //		DecideGps00().lidarDistance = obsPoint.y;
+                        return obsPoint;
+                    }
                 }
             }
 
@@ -681,10 +702,29 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                     {
                         count++; obsPoint.x = ptx; obsPoint.y = pty;
                     }
+
+                    if (count >= 2)
+                    {
+                        obsPoint.x = gpsTrace[j].x;
+                        obsPoint.y = gpsTrace[j].y;
+
+                        //int dx = ( obsPoint.x + gridwide*(double)centerx)/gridwide;
+                        //int dy = (obsPoint.y + gridwide*(double)centery+xiuzheng)/gridwide;
+
+                        obsPoint.obs_speed_x=lidarGridPtr[dx * (iv::gry) + dy].speed_x;
+                        obsPoint.obs_speed_y=lidarGridPtr[dx * (iv::gry) + dy].speed_y;
+                        obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
+
+                        isRemove = true;
+                        givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+                                      obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
+                        //		DecideGps00().lidarDistance = obsPoint.y;
+                        return obsPoint;
+                    }
                 }
             }
 
-            if (count >= 2)
+            /*if (count >= 2)
             {
                 obsPoint.x = gpsTrace[j].x;
                 obsPoint.y = gpsTrace[j].y;
@@ -697,9 +737,11 @@ iv::Point2D iv::decition::Compute00::getLidarObsPoint(std::vector<Point2D> gpsTr
                 obsPoint.obs_type= lidarGridPtr[dx * (iv::gry) + dy].type;
 
                 isRemove = true;
+                givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+                              obsPoint.y,lidarGridPtr[dx * (iv::gry) + dy].speed_y);
                 //		DecideGps00().lidarDistance = obsPoint.y;
                 return obsPoint;
-            }
+            }*/
         }
     }
     //	DecideGps00().lidarDistance = obsPoint.y;

src/decition/decition_brain_sf/decition/brain.cpp

@@ -35,7 +35,7 @@ iv::decition::BrainDecition * gbrain;
             gbrain->UpdateSate();
         }
 
-        void ListenHMI(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+        void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             gbrain->UpdateHMI(strdata,nSize);
         }
@@ -50,6 +50,11 @@ iv::decition::BrainDecition * gbrain;
             gbrain->UpdateGroupInfo(strdata,nSize);
         }
 
+        void ListenFusion(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+        {
+           gbrain->GetFusion(strdata,nSize);
+        }
+
     /*    void ListenMap_change_req(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             iv::formation_map_index::map map;
@@ -105,6 +110,8 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
 
+    mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
+
 
     mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
     mpaDecition = iv::modulecomm::RegisterSend(gstrmemdecition.data(),10*sizeof(iv::decition::DecitionBasic),1);
@@ -674,8 +681,15 @@ void iv::decition::BrainDecition::run() {
                 mfVehSpeedLast=ServiceCarStatus.vehSpeed_st;
             }
 
-            iv::LidarGridPtr templidar= obs_lidar_grid_;
+//            iv::LidarGridPtr templidar= obs_lidar_grid_;
+
+
+            iv::LidarGridPtr  templidar = fusion_ptr_;
+
+
             if(templidar == NULL)templidar = old_obs_lidar_grid_;
+
+
         //    GaussProjCal(gps_data_->gps_lng,gps_data_->gps_lat,&gps_data_->gps_x,&gps_data_->gps_y);
 
 
@@ -710,6 +724,7 @@ void iv::decition::BrainDecition::run() {
 
             mMutexMap.lock();
             decition_gps = decitionGps00->getDecideFromGPS(*gps_data_, navigation_data, templidar,*lidar_per,trafficLight);  // my dicition=============================
+//            free(templidar);
             mMutexMap.unlock();
 
             if(mbUseExternMPC)
@@ -1306,6 +1321,78 @@ void iv::decition::BrainDecition::UpdateChassis(const char *strdata, const unsig
     //    }
 }
 
+void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasize)
+{
+    std::shared_ptr<iv::fusion::fusionobjectarray> xfusion (new iv::fusion::fusionobjectarray);
+
+    if(ndatasize<1)return;
+    if(false == xfusion->ParseFromArray(pdata,ndatasize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::cout<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+
+    iv::decition::BrainDecition::UpdateFusion(xfusion);
+}
+
+void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
+{
+    mMutex_.lock();
+    fusion_obs.swap(mfusion_obs_);
+
+    fusion_ptr_ = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
+    memset(fusion_ptr_,0,sizeof(Obs_grid[iv::grx][iv::gry]));
+    for(int i =0; i<iv::grx; i++)     //复制到指针数组
+    {
+        for(int j =0; j<iv::gry; j++)
+        {
+            fusion_ptr_[i*(iv::gry)+j].ob = 0;
+        }
+
+    }
+
+    for(int i =0; i<mfusion_obs_->obj_size();i++)
+    {
+        iv::fusion::fusionobject xobs = mfusion_obs_->obj(i);
+        for(int j =0; j<xobs.nomal_centroid().size(); j++)
+        {
+            int dx, dy;
+            dx = (xobs.nomal_centroid(j).nomal_x() + gridwide*(double)centerx)/gridwide;
+            dy = (xobs.nomal_centroid(j).nomal_y() + gridwide*(double)centery)/gridwide;
+
+
+            if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
+            {
+                fusion_ptr_[dx*(iv::gry) + dy].id = xobs.id();
+                fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
+                fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
+                fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
+                fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
+                fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
+
+                if(xobs.centroid().y() < 50 && xobs.centroid().y() >5 )
+                {
+                    std::cout<<"     x       y    vy "<<xobs.centroid().x() <<  "     "
+                            << xobs.centroid().y()<< "     "<<fusion_ptr_[dx*(iv::gry) + dy].speed_y<< "     "
+                            <<xobs.vel_relative().y()<<std::endl;
+                    givlog->debug("brain_decition","SendobsDistance: %f,SendobsSpeed: %f",
+                                  xobs.centroid().y(),fusion_ptr_[dx*(iv::gry) + dy].speed_y);
+
+                }
+            }
+        }
+
+    }
+    mMutex_.unlock();
+
+}
+
+
 void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int ndatasize){
 
     iv::radio::radio_send group_message;

src/decition/decition_brain_sf/decition/brain.h

@@ -8,6 +8,7 @@
 
 #include <common/boost.h>
 #include <common/gps_type.h>
+#include <common/obstacle_type.h>
 //#include <control/controller.h>
 //#include <control/can.h>
 #include <perception/eyes.h>
@@ -24,6 +25,7 @@
 #include<perception/sensor_radar.h>
 //#include <server/carcalling.h>
 //#include "adcivstatedb.h"
+#include <fusionobjectarray.pb.h>
 
 //#include "decition/vehiclestate_type.h"
 #include "common/hmi_type.h"
@@ -127,6 +129,7 @@ namespace iv {
             void * mpamapreq;
             void * mpa;
             void * mpvbox;
+            void * mpfusion;
             QMutex mMutexMap;
 
             void * mpaDecition;
@@ -157,6 +160,14 @@ namespace iv {
             void UpdateGroupInfo(const char * pdata,const int ndatasize);
             void UpdateSate();
             void UpdateVbox(const char * pdata,const int ndatasize);
+            void GetFusion(const char* pdata, const int ndatasize);
+            void UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs);
+
+        private:
+            std::shared_ptr<iv::fusion::fusionobjectarray>  mfusion_obs_;
+            iv::LidarGridPtr fusion_ptr_;
+            QMutex mMutex_;
+
 
         private:
 //            Adcivstatedb * mpasd;

src/decition/decition_brain_sf/decition/decide_gps_00.cpp

@@ -1414,6 +1414,9 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
 
 
+        //givlog->debug("brain_decition","obsDistance: %f,obsSpeed: %f",
+                      //obsDistance,obsSpeed);
+
 
         if(obs_filter_flag==0)
         {
@@ -1460,8 +1463,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         obs_speed_for_avoid=secSpeed+obsSpeed;
 
-        givlog->debug("brain_decition","obsDistance: %f,obsSpeedforAvoid: %f",
-                      obsDistance,obs_speed_for_avoid);
+        //givlog->debug("brain_decition","obsDistance: %f,obsSpeedforAvoid: %f",
+                      //obsDistance,obs_speed_for_avoid);
 
     }else{
         gpsTraceRear.clear();
@@ -2069,7 +2072,6 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     lastAngle=gps_decition->wheel_angle;
 
     //   qDebug("decide1 time is %d",xTime.elapsed());
-
     return gps_decition;
 }