i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

MapCloud: auto refresh clouds when rtabmap is reset

Browse Source
This commit is contained in:
matlabbe
2016-11-09 16:21:06 -05:00
parent 2c326714ec
commit 8eb164f047
1 changed files with 42 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/rviz/MapCloudDisplay.cpp
+4 -5
View File
@@ -272,10 +272,8 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i) for(unsigned int i=0; i<map.nodes.size() && i<map.nodes.size(); ++i)
{ {
int id = map.nodes[i].id; int id = map.nodes[i].id;
if(poses.find(id) != poses.end() &&
cloud_infos_.find(id) == cloud_infos_.end()) // Always refresh the cloud if there are data
{
// Cloud not added to RVIZ, add it!
rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]); rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!s.sensorData().imageCompressed().empty() && if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() && !s.sensorData().depthOrRightCompressed().empty() &&
@@ -322,13 +320,13 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if (transformCloud(info, true)) if (transformCloud(info, true))
{ {
boost::mutex::scoped_lock lock(new_clouds_mutex_); boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.erase(id);
new_cloud_infos_.insert(std::make_pair(id, info)); new_cloud_infos_.insert(std::make_pair(id, info));
} }
} }
} }
} }
} }
}
// Update graph // Update graph
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -633,6 +631,7 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() ); cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
cloud_info->scene_node_->setVisible(false); cloud_info->scene_node_->setVisible(false);
cloud_infos_.erase(it->first);
cloud_infos_.insert(*it); cloud_infos_.insert(*it);
} }