12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Do not include trimmed submaps into the occupancy grid (#591) 

Derive submap IDs that don't appear in the submap list anymore and delete the corresponding slices from our state before drawing.
master
Michael Grupp 2017-11-14 14:49:19 +01:00 committed by Wally B. Feed
parent 9fb2b09a0b
commit b56903c1bb
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
cartographer_ros/cartographer_ros/occupancy_grid_node_main.cc
View File

@ -103,8 +103,16 @@ void Node::HandleSubmapList(
if (occupancy_grid_publisher_.getNumSubscribers() == 0) { if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
return; return;
} }
// Keep track of submap IDs that don't appear in the message anymore.
std::set<SubmapId> submap_ids_to_delete;
for (const auto& pair : submap_slices_) {
submap_ids_to_delete.insert(pair.first);
}
for (const auto& submap_msg : msg->submap) { for (const auto& submap_msg : msg->submap) {
const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index}; const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
submap_ids_to_delete.erase(id);
SubmapSlice& submap_slice = submap_slices_[id]; SubmapSlice& submap_slice = submap_slices_[id];
submap_slice.pose = ToRigid3d(submap_msg.pose); submap_slice.pose = ToRigid3d(submap_msg.pose);
submap_slice.metadata_version = submap_msg.submap_version; submap_slice.metadata_version = submap_msg.submap_version;
@ -158,6 +166,12 @@ void Node::HandleSubmapList(
<< cairo_status_to_string( << cairo_status_to_string(
cairo_surface_status(submap_slice.surface.get())); cairo_surface_status(submap_slice.surface.get()));
} }
// Delete all submaps that didn't appear in the message.
for (const auto& id : submap_ids_to_delete) {
submap_slices_.erase(id);
}
last_timestamp_ = msg->header.stamp; last_timestamp_ = msg->header.stamp;
last_frame_id_ = msg->header.frame_id; last_frame_id_ = msg->header.frame_id;
} }