Add toggle for pose markers to submaps display RViz plugin. (#1012)

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -48,7 +48,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
                                ::rviz::DisplayContext* const display_context,
                                Ogre::SceneNode* const map_node,
                                ::rviz::Property* const submap_category,
-                               const bool visible, const float pose_axes_length,
+                               const bool visible, const bool pose_axes_visible,
+                               const float pose_axes_length,
                                const float pose_axes_radius)
     : id_(id),
       display_context_(display_context),
@@ -56,6 +57,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
       submap_id_text_node_(submap_node_->createChildSceneNode()),
       pose_axes_(display_context->getSceneManager(), submap_node_,
                  pose_axes_length, pose_axes_radius),
+      pose_axes_visible_(pose_axes_visible),
       submap_id_text_(QString("(%1,%2)")
                           .arg(id.trajectory_id)
                           .arg(id.submap_index)
@@ -77,7 +79,6 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
   submap_id_text_.setColor(kSubmapIdColor);
   submap_id_text_.setTextAlignment(::rviz::MovableText::H_CENTER,
                                    ::rviz::MovableText::V_ABOVE);
-  // TODO(jihoonl): Make it toggleable.
   submap_id_text_node_->setPosition(ToOgre(kSubmapIdPosition));
   submap_id_text_node_->attachObject(&submap_id_text_);
   connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
@@ -192,4 +193,9 @@ void DrawableSubmap::ToggleVisibility() {
   display_context_->queueRender();
 }
 
+void DrawableSubmap::TogglePoseMarkerVisibility() {
+  submap_id_text_node_->setVisible(pose_axes_visible_);
+  pose_axes_.getSceneNode()->setVisible(pose_axes_visible_);
+}
+
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/drawable_submap.h

@@ -50,7 +50,8 @@ class DrawableSubmap : public QObject {
   DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id,
                  ::rviz::DisplayContext* display_context,
                  Ogre::SceneNode* map_node, ::rviz::Property* submap_category,
-                 bool visible, float pose_axes_length, float pose_axes_radius);
+                 bool visible, const bool pose_axes_visible,
+                 float pose_axes_length, float pose_axes_radius);
   ~DrawableSubmap() override;
   DrawableSubmap(const DrawableSubmap&) = delete;
   DrawableSubmap& operator=(const DrawableSubmap&) = delete;
@@ -83,6 +84,10 @@ class DrawableSubmap : public QObject {
   void set_visibility(const bool visibility) {
     visibility_->setBool(visibility);
   }
+  void set_pose_markers_visibility(const bool visibility) {
+    pose_axes_visible_ = visibility;
+    TogglePoseMarkerVisibility();
+  }
 
  Q_SIGNALS:
   // RPC request succeeded.
@@ -92,6 +97,7 @@ class DrawableSubmap : public QObject {
   // Callback when an rpc request succeeded.
   void UpdateSceneNode();
   void ToggleVisibility();
+  void TogglePoseMarkerVisibility();
 
  private:
   const ::cartographer::mapping::SubmapId id_;
@@ -103,6 +109,7 @@ class DrawableSubmap : public QObject {
   std::vector<std::unique_ptr<OgreSlice>> ogre_slices_;
   ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
   ::rviz::Axes pose_axes_;
+  bool pose_axes_visible_;
   ::rviz::MovableText submap_id_text_;
   std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
   bool query_in_progress_ GUARDED_BY(mutex_) = false;

cartographer_rviz/cartographer_rviz/submaps_display.cc

@@ -65,6 +65,10 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) {
       "All", true,
       "Whether submaps from all trajectories should be displayed or not.",
       trajectories_category_, SLOT(AllEnabledToggled()), this);
+  pose_markers_all_enabled_ = new ::rviz::BoolProperty(
+      "All Submap Pose Markers", true,
+      "Whether submap pose markers should be displayed or not.",
+      trajectories_category_, SLOT(PoseMarkersEnabledToggled()), this);
   fade_out_start_distance_in_meters_ =
       new ::rviz::FloatProperty("Fade-out distance", 1.f,
                                 "Distance in meters in z-direction beyond "
@@ -135,18 +139,21 @@ 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(
+      trajectories_.push_back(absl::make_unique<Trajectory>(
-          absl::make_unique<Trajectory>(absl::make_unique<::rviz::BoolProperty>(
+          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),
-              trajectories_category_)));
+              trajectories_category_),
+          pose_markers_all_enabled_->getBool()));
     }
     auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility;
     auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps;
+    auto& pose_markers_visibility =
+        trajectories_[id.trajectory_id]->pose_markers_visibility;
     if (trajectory_submaps.count(id.submap_index) == 0) {
       // TODO(ojura): Add RViz properties for adjusting submap pose axes
       constexpr float kSubmapPoseAxesLength = 0.3f;
@@ -155,7 +162,8 @@ void SubmapsDisplay::processMessage(
           id.submap_index,
           absl::make_unique<DrawableSubmap>(
               id, context_, map_node_, trajectory_visibility.get(),
-              trajectory_visibility->getBool(), kSubmapPoseAxesLength,
+              trajectory_visibility->getBool(),
+              pose_markers_visibility->getBool(), kSubmapPoseAxesLength,
               kSubmapPoseAxesRadius));
       trajectory_submaps.at(id.submap_index)
           ->SetSliceVisibility(0, slice_high_resolution_enabled_->getBool());
@@ -237,6 +245,14 @@ void SubmapsDisplay::AllEnabledToggled() {
   }
 }
 
+void SubmapsDisplay::PoseMarkersEnabledToggled() {
+  absl::MutexLock locker(&mutex_);
+  const bool visible = pose_markers_all_enabled_->getBool();
+  for (auto& trajectory : trajectories_) {
+    trajectory->pose_markers_visibility->setBool(visible);
+  }
+}
+
 void SubmapsDisplay::ResolutionToggled() {
   absl::MutexLock locker(&mutex_);
   for (auto& trajectory : trajectories_) {
@@ -256,10 +272,26 @@ void Trajectory::AllEnabledToggled() {
   }
 }
 
-Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property)
+void Trajectory::PoseMarkersEnabledToggled() {
+  const bool visible = pose_markers_visibility->getBool();
+  for (auto& submap_entry : submaps) {
+    submap_entry.second->set_pose_markers_visibility(visible);
+  }
+}
+
+Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property,
+                       const bool pose_markers_enabled)
     : visibility(std::move(property)) {
   ::QObject::connect(visibility.get(), SIGNAL(changed()), this,
                      SLOT(AllEnabledToggled()));
+  // Add toggle for submap pose markers as the first entry of the visibility
+  // property list of this trajectory.
+  pose_markers_visibility = absl::make_unique<::rviz::BoolProperty>(
+      QString("Submap Pose Markers"), pose_markers_enabled,
+      QString("Toggles the submap pose markers of this trajectory."),
+      visibility.get());
+  ::QObject::connect(pose_markers_visibility.get(), SIGNAL(changed()), this,
+                     SLOT(PoseMarkersEnabledToggled()));
 }
 
 }  // namespace cartographer_rviz

cartographer_rviz/cartographer_rviz/submaps_display.h

@@ -41,13 +41,16 @@ struct Trajectory : public QObject {
   Q_OBJECT
 
  public:
-  Trajectory(std::unique_ptr<::rviz::BoolProperty> property);
+  Trajectory(std::unique_ptr<::rviz::BoolProperty> property,
+             bool pose_markers_enabled);
 
   std::unique_ptr<::rviz::BoolProperty> visibility;
+  std::unique_ptr<::rviz::BoolProperty> pose_markers_visibility;
   std::map<int, std::unique_ptr<DrawableSubmap>> submaps;
 
  private Q_SLOTS:
   void AllEnabledToggled();
+  void PoseMarkersEnabledToggled();
 };
 
 // RViz plugin used for displaying maps which are represented by a collection of
@@ -70,6 +73,7 @@ class SubmapsDisplay
  private Q_SLOTS:
   void Reset();
   void AllEnabledToggled();
+  void PoseMarkersEnabledToggled();
   void ResolutionToggled();
 
  private:
@@ -95,6 +99,7 @@ class SubmapsDisplay
   ::rviz::BoolProperty* slice_low_resolution_enabled_;
   ::rviz::Property* trajectories_category_;
   ::rviz::BoolProperty* visibility_all_enabled_;
+  ::rviz::BoolProperty* pose_markers_all_enabled_;
   ::rviz::FloatProperty* fade_out_start_distance_in_meters_;
 };