diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 06b9f75..c984b2e 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/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 diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index e2f9e43..21e1933 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/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> 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; diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 7cfcb96..da02ccb 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/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(trajectories_.size())) { - trajectories_.push_back( - absl::make_unique(absl::make_unique<::rviz::BoolProperty>( + trajectories_.push_back(absl::make_unique( + 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( 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 diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 4c05e79..d959027 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/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> 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_; };