Remove deleted trajectories from submaps display. (#1065)

Fixes strange behavior with empty leftover entries in the trajectory list of the RViz plugin.

By using an `std::map` instead of an `std::vector` for the trajectories  we can
easily erase trajectories which do not appear in the submap list anymore.

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -121,7 +121,7 @@ void SubmapsDisplay::processMessage(
   // previous instance.
   for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
     const size_t trajectory_id = submap_entry.trajectory_id;
-    if (trajectory_id >= trajectories_.size()) {
+    if (trajectories_.count(trajectory_id) == 0) {
       continue;
     }
     const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
@@ -135,20 +135,25 @@ void SubmapsDisplay::processMessage(
   }
   using ::cartographer::mapping::SubmapId;
   std::set<SubmapId> listed_submaps;
+  std::set<int> listed_trajectories;
   for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) {
     const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
     listed_submaps.insert(id);
-    while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
+    listed_trajectories.insert(submap_entry.trajectory_id);
-      trajectories_.push_back(absl::make_unique<Trajectory>(
+    if (trajectories_.count(id.trajectory_id) == 0) {
-          absl::make_unique<::rviz::BoolProperty>(
+      trajectories_.insert(std::make_pair(
-              QString("Trajectory %1").arg(id.trajectory_id),
+          id.trajectory_id,
-              visibility_all_enabled_->getBool(),
+          absl::make_unique<Trajectory>(
-              QString("List of all submaps in Trajectory %1. The checkbox "
+              absl::make_unique<::rviz::BoolProperty>(
+                  QString("Trajectory %1").arg(id.trajectory_id),
+                  visibility_all_enabled_->getBool(),
+                  QString(
+                      "List of all submaps in Trajectory %1. The checkbox "
                       "controls whether all submaps in this trajectory should "
                       "be displayed or not.")
-                  .arg(id.trajectory_id),
+                      .arg(id.trajectory_id),
-              trajectories_category_),
+                  trajectories_category_),
-          pose_markers_all_enabled_->getBool()));
+              pose_markers_all_enabled_->getBool())));
     }
     auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
     auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
@@ -172,10 +177,18 @@ void SubmapsDisplay::processMessage(
     }
     trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
   }
+  // Remove all deleted trajectories not mentioned in the SubmapList.
+  for (auto it = trajectories_.begin(); it != trajectories_.end();) {
+    if (listed_trajectories.count(it->first) == 0) {
+      it = trajectories_.erase(it);
+    } else {
+      ++it;
+    }
+  }
   // Remove all submaps not mentioned in the SubmapList.
-  for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
+  for (const auto& trajectory_by_id : trajectories_) {
-       ++trajectory_id) {
+    const int trajectory_id = trajectory_by_id.first;
-    auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
+    auto& trajectory_submaps = trajectory_by_id.second->submaps;
     for (auto it = trajectory_submaps.begin();
          it != trajectory_submaps.end();) {
       if (listed_submaps.count(
@@ -191,15 +204,15 @@ void SubmapsDisplay::processMessage(
 void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   absl::MutexLock locker(&mutex_);
   // Schedule fetching of new submap textures.
-  for (const auto& trajectory : trajectories_) {
+  for (const auto& trajectory_by_id : trajectories_) {
     int num_ongoing_requests = 0;
-    for (const auto& submap_entry : trajectory->submaps) {
+    for (const auto& submap_entry : trajectory_by_id.second->submaps) {
       if (submap_entry.second->QueryInProgress()) {
         ++num_ongoing_requests;
       }
     }
-    for (auto it = trajectory->submaps.rbegin();
+    for (auto it = trajectory_by_id.second->submaps.rbegin();
-         it != trajectory->submaps.rend() &&
+         it != trajectory_by_id.second->submaps.rend() &&
          num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
          ++it) {
       if (it->second->MaybeFetchTexture(&client_)) {
@@ -216,8 +229,8 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
     const ::geometry_msgs::TransformStamped transform_stamped =
         tf_buffer_.lookupTransform(
             *map_frame_, tracking_frame_property_->getStdString(), kLatest);
-    for (auto& trajectory : trajectories_) {
+    for (auto& trajectory_by_id : trajectories_) {
-      for (auto& submap_entry : trajectory->submaps) {
+      for (auto& submap_entry : trajectory_by_id.second->submaps) {
         submap_entry.second->SetAlpha(
             transform_stamped.transform.translation.z,
             fade_out_start_distance_in_meters_->getFloat());
@@ -240,23 +253,23 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
 void SubmapsDisplay::AllEnabledToggled() {
   absl::MutexLock locker(&mutex_);
   const bool visible = visibility_all_enabled_->getBool();
-  for (auto& trajectory : trajectories_) {
+  for (auto& trajectory_by_id : trajectories_) {
-    trajectory->visibility->setBool(visible);
+    trajectory_by_id.second->visibility->setBool(visible);
   }
 }
 
 void SubmapsDisplay::PoseMarkersEnabledToggled() {
   absl::MutexLock locker(&mutex_);
   const bool visible = pose_markers_all_enabled_->getBool();
-  for (auto& trajectory : trajectories_) {
+  for (auto& trajectory_by_id : trajectories_) {
-    trajectory->pose_markers_visibility->setBool(visible);
+    trajectory_by_id.second->pose_markers_visibility->setBool(visible);
   }
 }
 
 void SubmapsDisplay::ResolutionToggled() {
   absl::MutexLock locker(&mutex_);
-  for (auto& trajectory : trajectories_) {
+  for (auto& trajectory_by_id : trajectories_) {
-    for (auto& submap_entry : trajectory->submaps) {
+    for (auto& submap_entry : trajectory_by_id.second->submaps) {
       submap_entry.second->SetSliceVisibility(
           0, slice_high_resolution_enabled_->getBool());
       submap_entry.second->SetSliceVisibility(

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -93,7 +93,7 @@ class SubmapsDisplay
   std::unique_ptr<std::string> map_frame_;
   ::rviz::StringProperty* tracking_frame_property_;
   Ogre::SceneNode* map_node_ = nullptr;  // Represents the map frame.
-  std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
+  std::map<int, std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
   absl::Mutex mutex_;
   ::rviz::BoolProperty* slice_high_resolution_enabled_;
   ::rviz::BoolProperty* slice_low_resolution_enabled_;