Add toggleable trajectories to RViz plugin (#456)

Implementation of #422 by @ojura. Original feature idea and implementation by @jihoonl.

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -53,13 +53,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
       "Tracking frame", kDefaultTrackingFrame,
       "Tracking frame, used for fading out submaps.", this);
   client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>("");
-  submaps_category_ = new ::rviz::Property(
+  trajectories_category_ = new ::rviz::Property(
       "Submaps", QVariant(), "List of all submaps, organized by trajectories.",
       this);
   visibility_all_enabled_ = new ::rviz::BoolProperty(
       "All Enabled", true,
-      "Whether all the submaps should be displayed or not.", submaps_category_,
+      "Whether submaps from all trajectories should be displayed or not.",
-      SLOT(AllEnabledToggled()), this);
+      trajectories_category_, SLOT(AllEnabledToggled()), this);
   const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME);
   Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
       package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME);
@@ -111,9 +111,9 @@ void SubmapsDisplay::processMessage(
     if (trajectory_id >= trajectories_.size()) {
       continue;
     }
-    const auto& trajectory = trajectories_[trajectory_id].second;
+    const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
-    const auto it = trajectory.find(submap_entry.submap_index);
+    const auto it = trajectory_submaps.find(submap_entry.submap_index);
-    if (it != trajectory.end() &&
+    if (it != trajectory_submaps.end() &&
         it->second->version() > submap_entry.submap_version) {
       // Versions should only increase unless Cartographer restarted.
       trajectories_.clear();
@@ -126,36 +126,40 @@ void SubmapsDisplay::processMessage(
     const SubmapId id{submap_entry.trajectory_id, submap_entry.submap_index};
     listed_submaps.insert(id);
     while (id.trajectory_id >= static_cast<int>(trajectories_.size())) {
-      trajectories_.push_back(Trajectory(
+      trajectories_.push_back(::cartographer::common::make_unique<Trajectory>(
-          ::cartographer::common::make_unique<::rviz::Property>(
+          ::cartographer::common::make_unique<::rviz::BoolProperty>(
-              QString("Trajectory %1").arg(id.trajectory_id), QVariant(),
+              QString("Trajectory %1").arg(id.trajectory_id),
-              QString("List of all submaps in Trajectory %1.")
+              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),
-              submaps_category_),
+              trajectories_category_)));
-          std::map<int, std::unique_ptr<DrawableSubmap>>()));
     }
-    auto& trajectory_category = trajectories_[id.trajectory_id].first;
+    auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
-    auto& trajectory = trajectories_[id.trajectory_id].second;
+    auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
-    if (trajectory.count(id.submap_index) == 0) {
+    if (trajectory_submaps.count(id.submap_index) == 0) {
       // TODO(ojura): Add RViz properties for adjusting submap pose axes
       constexpr float kSubmapPoseAxesLength = 0.3f;
       constexpr float kSubmapPoseAxesRadius = 0.06f;
-      trajectory.emplace(id.submap_index,
+      trajectory_submaps.emplace(
-                         ::cartographer::common::make_unique<DrawableSubmap>(
+          id.submap_index,
-                             id, context_, map_node_, trajectory_category.get(),
+          ::cartographer::common::make_unique<DrawableSubmap>(
-                             visibility_all_enabled_->getBool(),
+              id, context_, map_node_, trajectory_visibility.get(),
-                             kSubmapPoseAxesLength, kSubmapPoseAxesRadius));
+              trajectory_visibility->getBool(), kSubmapPoseAxesLength,
+              kSubmapPoseAxesRadius));
     }
-    trajectory.at(id.submap_index)->Update(msg->header, submap_entry);
+    trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry);
   }
   // Remove all submaps not mentioned in the SubmapList.
   for (size_t trajectory_id = 0; trajectory_id < trajectories_.size();
        ++trajectory_id) {
-    auto& trajectory = trajectories_[trajectory_id].second;
+    auto& trajectory_submaps = trajectories_[trajectory_id]->submaps;
-    for (auto it = trajectory.begin(); it != trajectory.end();) {
+    for (auto it = trajectory_submaps.begin();
+         it != trajectory_submaps.end();) {
       if (listed_submaps.count(
               SubmapId{static_cast<int>(trajectory_id), it->first}) == 0) {
-        it = trajectory.erase(it);
+        it = trajectory_submaps.erase(it);
       } else {
         ++it;
       }
@@ -168,13 +172,13 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
   // Schedule fetching of new submap textures.
   for (const auto& trajectory : trajectories_) {
     int num_ongoing_requests = 0;
-    for (const auto& submap_entry : trajectory.second) {
+    for (const auto& submap_entry : trajectory->submaps) {
       if (submap_entry.second->QueryInProgress()) {
         ++num_ongoing_requests;
       }
     }
-    for (auto it = trajectory.second.rbegin();
+    for (auto it = trajectory->submaps.rbegin();
-         it != trajectory.second.rend() &&
+         it != trajectory->submaps.rend() &&
          num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory;
          ++it) {
       if (it->second->MaybeFetchTexture(&client_)) {
@@ -192,7 +196,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
         tf_buffer_.lookupTransform(
             *map_frame_, tracking_frame_property_->getStdString(), kLatest);
     for (auto& trajectory : trajectories_) {
-      for (auto& submap_entry : trajectory.second) {
+      for (auto& submap_entry : trajectory->submaps) {
         submap_entry.second->SetAlpha(
             transform_stamped.transform.translation.z);
       }
@@ -213,14 +217,25 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
 
 void SubmapsDisplay::AllEnabledToggled() {
   ::cartographer::common::MutexLocker locker(&mutex_);
-  const bool visibility = visibility_all_enabled_->getBool();
+  const bool visible = visibility_all_enabled_->getBool();
   for (auto& trajectory : trajectories_) {
-    for (auto& submap_entry : trajectory.second) {
+    trajectory->visibility->setBool(visible);
-      submap_entry.second->set_visibility(visibility);
-    }
   }
 }
 
+void Trajectory::AllEnabledToggled() {
+  const bool visible = visibility->getBool();
+  for (auto& submap_entry : submaps) {
+    submap_entry.second->set_visibility(visible);
+  }
+}
+
+Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property)
+    : visibility(std::move(property)) {
+  ::QObject::connect(visibility.get(), SIGNAL(changed()), this,
+                     SLOT(AllEnabledToggled()));
+}
+
 }  // namespace cartographer_rviz
 
 PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display)

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -32,6 +32,21 @@
 
 namespace cartographer_rviz {
 
+// This should be a private class in SubmapsDisplay, unfortunately, QT does not
+// allow for this.
+struct Trajectory : public QObject {
+  Q_OBJECT
+
+ public:
+  Trajectory(std::unique_ptr<::rviz::BoolProperty> property);
+
+  std::unique_ptr<::rviz::BoolProperty> visibility;
+  std::map<int, std::unique_ptr<DrawableSubmap>> submaps;
+
+ private Q_SLOTS:
+  void AllEnabledToggled();
+};
+
 // RViz plugin used for displaying maps which are represented by a collection of
 // submaps.
 //
@@ -70,11 +85,9 @@ class SubmapsDisplay
   std::unique_ptr<std::string> map_frame_;
   ::rviz::StringProperty* tracking_frame_property_;
   Ogre::SceneNode* map_node_ = nullptr;  // Represents the map frame.
-  using Trajectory = std::pair<std::unique_ptr<::rviz::Property>,
+  std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
-                               std::map<int, std::unique_ptr<DrawableSubmap>>>;
-  std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
   ::cartographer::common::Mutex mutex_;
-  ::rviz::Property* submaps_category_;
+  ::rviz::Property* trajectories_category_;
   ::rviz::BoolProperty* visibility_all_enabled_;
 };