|
@@ -485,13 +485,13 @@ void iv::decition::BrainDecition::run() {
|
|
|
// gps_data_ = NULL;
|
|
|
|
|
|
// if(obs_lidar_grid_ != NULL)free(obs_lidar_grid_);
|
|
|
- if(obs_lidar_grid_ != NULL)
|
|
|
- {
|
|
|
- if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
|
|
|
- old_obs_lidar_grid_ = obs_lidar_grid_;
|
|
|
- }
|
|
|
+// if(fusion_ptr_ != NULL)
|
|
|
+// {
|
|
|
+// if(old_obs_lidar_grid_ != NULL) free(old_obs_lidar_grid_);
|
|
|
+// old_obs_lidar_grid_ = fusion_ptr_;
|
|
|
+// }
|
|
|
|
|
|
- obs_lidar_grid_ = NULL;
|
|
|
+// fusion_ptr_ = NULL;
|
|
|
|
|
|
if(obs_camera_ != NULL)free(obs_camera_);
|
|
|
|
|
@@ -682,40 +682,28 @@ void iv::decition::BrainDecition::run() {
|
|
|
}
|
|
|
|
|
|
// iv::LidarGridPtr templidar= obs_lidar_grid_;
|
|
|
-
|
|
|
iv::LidarGridPtr templidar;
|
|
|
- static int nUseOldCount = 0;
|
|
|
+ templidar = NULL;
|
|
|
mMutex_.lock();
|
|
|
- templidar = fusion_ptr_;
|
|
|
- fusion_ptr_ = NULL;
|
|
|
- mMutex_.unlock();
|
|
|
|
|
|
- if(templidar != NULL)
|
|
|
+ if(fusion_ptr_ != NULL)
|
|
|
{
|
|
|
- if(old_fusion_grid_ != NULL)free(old_fusion_grid_);
|
|
|
- old_fusion_grid_ = templidar;
|
|
|
- nUseOldCount = 0;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- if((old_fusion_grid_ != NULL)&&(nUseOldCount < 300))
|
|
|
- {
|
|
|
- templidar = old_fusion_grid_;
|
|
|
- nUseOldCount++;
|
|
|
- }
|
|
|
- }
|
|
|
-// if (fusion_ptr_ != NULL)
|
|
|
-// {
|
|
|
-// if(templidar != NULL) free(templidar);
|
|
|
-// templidar = fusion_ptr_;
|
|
|
-// }
|
|
|
-// fusion_ptr_ =NULL;
|
|
|
+ if(old_obs_lidar_grid_ != NULL)
|
|
|
+ {
|
|
|
+ free(old_obs_lidar_grid_);
|
|
|
+ }
|
|
|
+ old_obs_lidar_grid_ = fusion_ptr_;
|
|
|
+ templidar = fusion_ptr_;
|
|
|
|
|
|
+ }
|
|
|
+ fusion_ptr_ = NULL;
|
|
|
+ mMutex_.unlock();
|
|
|
|
|
|
// iv::LidarGridPtr templidar = fusion_ptr_;
|
|
|
|
|
|
|
|
|
- if(templidar == NULL)templidar = old_obs_lidar_grid_;
|
|
|
+ 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);
|
|
@@ -1368,9 +1356,14 @@ void iv::decition::BrainDecition::GetFusion(const char *pdata, const int ndatasi
|
|
|
|
|
|
void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusionobjectarray> fusion_obs)
|
|
|
{
|
|
|
+
|
|
|
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++) //复制到指针数组
|