|
|
@@ -58,6 +58,29 @@ iv::decition::BrainDecition * gbrain;
|
|
|
gbrain->GetFusion(strdata,nSize);
|
|
|
}
|
|
|
|
|
|
+ void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
+ {
|
|
|
+
|
|
|
+ // qDebug("size is %d",nSize);
|
|
|
+
|
|
|
+ std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
|
|
|
+
|
|
|
+
|
|
|
+ iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
|
|
|
+ int nCount = nSize/sizeof(iv::ObstacleBasic);
|
|
|
+ int i;
|
|
|
+ for(i=0;i<nCount;i++)
|
|
|
+ {
|
|
|
+ iv::ObstacleBasic temp;
|
|
|
+ memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
|
|
|
+ lidar_obs->push_back(temp);
|
|
|
+ pdata++;
|
|
|
+ }
|
|
|
+ gbrain->UpdateOBS(lidar_obs);
|
|
|
+
|
|
|
+ // gw->UpdateOBS(lidar_obs);
|
|
|
+ }
|
|
|
+
|
|
|
/* 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;
|
|
|
@@ -113,7 +136,8 @@ iv::decition::BrainDecition::BrainDecition()
|
|
|
|
|
|
mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
|
|
|
|
|
|
- mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
|
|
|
+ mpfusion = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);
|
|
|
+// mpfusion = iv::modulecomm::RegisterRecv("li_ra_fusion",ListenFusion);
|
|
|
|
|
|
|
|
|
mpaToPlatform = iv::modulecomm::RegisterSend("toplatform",10*sizeof(iv::platform::PlatFormMsg),10);
|
|
|
@@ -1402,6 +1426,99 @@ void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasi
|
|
|
|
|
|
iv::decition::BrainDecition::UpdateFusion(xfusion);
|
|
|
}
|
|
|
+void iv::decition::BrainDecition::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
|
|
|
+{
|
|
|
+ std::shared_ptr<std::vector<iv::ObstacleBasic>> mobs;
|
|
|
+ iv::LidarGridPtr ptr;
|
|
|
+ mMutex_.lock();
|
|
|
+ lidar_obs.swap(mobs);
|
|
|
+ mMutex_.unlock();
|
|
|
+
|
|
|
+// ptr = (Obs_grid *)malloc(sizeof(Obs_grid[iv::grx][iv::gry]));
|
|
|
+// memset(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++)
|
|
|
+// {
|
|
|
+//// ptr[i * (iv::gry) + j].high = obs_grid[i][j].high;
|
|
|
+//// ptr[i * (iv::gry) + j].low = obs_grid[i][j].low;
|
|
|
+// ptr[i * (iv::gry) + j].ob = 0;
|
|
|
+//// ptr[i * (iv::gry) + j].obshight = obs_grid[i][j].obshight;
|
|
|
+//// ptr[i * (iv::gry) + j].pointcount = obs_grid[i][j].pointcount;
|
|
|
+// }
|
|
|
+// }
|
|
|
+// for(int i=0;i<mobs->size();i++)
|
|
|
+// {
|
|
|
+// iv::ObstacleBasic xobs = mobs->at(i);
|
|
|
+// int dx,dy;
|
|
|
+// dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
|
|
|
+// dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
|
|
|
+// if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
|
|
|
+// {
|
|
|
+// ptr[dx*(iv::gry) +dy].high = xobs.high;
|
|
|
+// ptr[dx*(iv::gry) +dy].low = xobs.low;
|
|
|
+// ptr[dx*(iv::gry) + dy].ob = 2;
|
|
|
+// ptr[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
|
|
|
+// ptr[dx*(iv::gry) + dy].pointcount = 5;
|
|
|
+// }
|
|
|
+
|
|
|
+// }
|
|
|
+// /* 激光雷达障碍物*/
|
|
|
+//// typedef void
|
|
|
+//// (sig_cb_lidar_sensor_obstacle_grid)(iv::LidarGridPtr);
|
|
|
+
|
|
|
+//// boost::signals2::signal<sig_cb_lidar_sensor_obstacle_grid>* signal_lidar_obstacle_grid; //激光雷达栅格
|
|
|
+
|
|
|
+//// signal_lidar_obstacle_grid->operator()(ptr);//触发
|
|
|
+
|
|
|
+// ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
|
|
|
+// int i;
|
|
|
+// int nSize = mobs->size();
|
|
|
+// for(i=0;i<nSize;i++)
|
|
|
+// _obs_grid_->push_back(mobs->at(i));
|
|
|
+// ServiceLidar.copyfromlidarobs(_obs_grid_);
|
|
|
+
|
|
|
+// ServiceCarStatus.mLidarS = 10;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ mMutex_.lock();
|
|
|
+// fusion_obs.swap(mfusion_obs_);
|
|
|
+ if(fusion_ptr_ != NULL)
|
|
|
+ {
|
|
|
+ free(fusion_ptr_);
|
|
|
+ fusion_ptr_ = NULL;
|
|
|
+ }
|
|
|
+ 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<mobs->size();i++)
|
|
|
+ {
|
|
|
+ iv::ObstacleBasic xobs = mobs->at(i);
|
|
|
+ int dx,dy;
|
|
|
+ dx = (xobs.nomal_x + gridwide * (double)centerx)/gridwide;
|
|
|
+ dy = (xobs.nomal_y + gridwide * (double)centery)/gridwide;
|
|
|
+ if((dx>=0)&&(dx<iv::grx)&&(dy>=0)&&(dy<iv::gry))
|
|
|
+ {
|
|
|
+ fusion_ptr_[dx*(iv::gry) +dy].high = xobs.high;
|
|
|
+ fusion_ptr_[dx*(iv::gry) +dy].low = xobs.low;
|
|
|
+ fusion_ptr_[dx*(iv::gry) + dy].ob = 2;
|
|
|
+ fusion_ptr_[dx*(iv::gry) + dy].obshight = xobs.nomal_z;
|
|
|
+ fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ mMutex_.unlock();
|
|
|
+}
|
|
|
|
|
|
void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
|
|
|
{
|
HAPO-9#