From bd28ec1a580d5f21345023f3005f9f2a6c78018e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 21 Jul 2017 11:52:25 +0200 Subject: [PATCH] Display submap poses in Submaps plugin (#416) - simplify code around toggling submap visibility - pass DisplayContext to DrawableSubmap - add parent scene node for both submap and axes - remove unnecessary const Fixes #405. --- .../cartographer_rviz/drawable_submap.cc | 47 ++++++++++--------- .../cartographer_rviz/drawable_submap.h | 13 +++-- .../cartographer_rviz/submaps_display.cc | 13 +++-- 3 files changed, 40 insertions(+), 33 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 8f2b56f..645d8ed 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -64,15 +64,21 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { } // namespace -DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, - Ogre::SceneManager* const scene_manager, - ::rviz::Property* submap_category, - const bool visible) +DrawableSubmap::DrawableSubmap( + const ::cartographer::mapping::SubmapId& id, + ::rviz::DisplayContext* const display_context, + ::rviz::Property* const submap_category, const bool visible, + const float pose_axes_length, const float pose_axes_radius) : id_(id), - scene_manager_(scene_manager), - scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()), - manual_object_(scene_manager->createManualObject( + display_context_(display_context), + scene_node_(display_context->getSceneManager() + ->getRootSceneNode() + ->createChildSceneNode()), + submap_node_(scene_node_->createChildSceneNode()), + manual_object_(display_context->getSceneManager()->createManualObject( kManualObjectPrefix + GetSubmapIdentifier(id))), + pose_axes_(display_context->getSceneManager(), scene_node_, + pose_axes_length, pose_axes_radius), last_query_timestamp_(0) { material_ = Ogre::MaterialManager::getSingleton().getByName( kSubmapSourceMaterialName); @@ -90,9 +96,8 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); - if (visible) { - scene_node_->attachObject(manual_object_); - } + submap_node_->attachObject(manual_object_); + scene_node_->setVisible(visible); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -107,8 +112,9 @@ DrawableSubmap::~DrawableSubmap() { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); texture_.setNull(); } - scene_manager_->destroySceneNode(scene_node_); - scene_manager_->destroyManualObject(manual_object_); + display_context_->getSceneManager()->destroySceneNode(submap_node_); + display_context_->getSceneManager()->destroySceneNode(scene_node_); + display_context_->getSceneManager()->destroyManualObject(manual_object_); } void DrawableSubmap::Update( @@ -250,8 +256,11 @@ void DrawableSubmap::UpdateTransform() { CHECK(submap_texture_ != nullptr); const ::cartographer::transform::Rigid3d pose = pose_ * submap_texture_->slice_pose; - scene_node_->setPosition(ToOgre(pose.translation())); - scene_node_->setOrientation(ToOgre(pose.rotation())); + submap_node_->setPosition(ToOgre(pose.translation())); + submap_node_->setOrientation(ToOgre(pose.rotation())); + pose_axes_.setPosition(ToOgre(pose_.translation())); + pose_axes_.setOrientation(ToOgre(pose_.rotation())); + display_context_->queueRender(); } float DrawableSubmap::UpdateAlpha(const float target_alpha) { @@ -263,15 +272,7 @@ float DrawableSubmap::UpdateAlpha(const float target_alpha) { } void DrawableSubmap::ToggleVisibility() { - if (visibility_->getBool()) { - if (scene_node_->numAttachedObjects() == 0) { - scene_node_->attachObject(manual_object_); - } - } else { - if (scene_node_->numAttachedObjects() > 0) { - scene_node_->detachObject(manual_object_); - } - } + scene_node_->setVisible(visibility_->getBool()); } } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 5c42a04..9e5a71b 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -38,6 +38,7 @@ #include "ros/ros.h" #include "rviz/display_context.h" #include "rviz/frame_manager.h" +#include "rviz/ogre_helpers/axes.h" #include "rviz/properties/bool_property.h" namespace cartographer_rviz { @@ -48,10 +49,10 @@ class DrawableSubmap : public QObject { Q_OBJECT public: - // The 'scene_manager' is the Ogre scene manager to which to add a node. DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, - Ogre::SceneManager* scene_manager, - ::rviz::Property* submap_category, const bool visible); + ::rviz::DisplayContext* display_context, + ::rviz::Property* submap_category, bool visible, + float pose_axes_length, float pose_axes_radius); ~DrawableSubmap() override; DrawableSubmap(const DrawableSubmap&) = delete; DrawableSubmap& operator=(const DrawableSubmap&) = delete; @@ -96,12 +97,14 @@ class DrawableSubmap : public QObject { const ::cartographer::mapping::SubmapId id_; ::cartographer::common::Mutex mutex_; - Ogre::SceneManager* const scene_manager_; + ::rviz::DisplayContext* const display_context_; Ogre::SceneNode* const scene_node_; - Ogre::ManualObject* manual_object_; + Ogre::SceneNode* const submap_node_; + Ogre::ManualObject* const manual_object_; Ogre::TexturePtr texture_; Ogre::MaterialPtr material_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); + ::rviz::Axes pose_axes_; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ = false GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_); diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index be4e65b..b8214af 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -133,11 +133,14 @@ void SubmapsDisplay::processMessage( auto& trajectory_category = trajectories_[id.trajectory_id].first; auto& trajectory = trajectories_[id.trajectory_id].second; if (trajectory.count(id.submap_index) == 0) { - trajectory.emplace( - id.submap_index, - ::cartographer::common::make_unique( - id, context_->getSceneManager(), trajectory_category.get(), - visibility_all_enabled_->getBool())); + // 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, + ::cartographer::common::make_unique( + id, context_, trajectory_category.get(), + visibility_all_enabled_->getBool(), + kSubmapPoseAxesLength, kSubmapPoseAxesRadius)); } trajectory.at(id.submap_index) ->Update(msg->header, submap_entry, context_->getFrameManager());