diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 97a8489..9d275e8 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -42,6 +42,10 @@ constexpr double kFadeOutStartDistanceInMeters = 1.; constexpr double kFadeOutDistanceInMeters = 2.; constexpr float kAlphaUpdateThreshold = 0.2f; +const Ogre::ColourValue kSubmapIdColor(Ogre::ColourValue::Red); +const Eigen::Vector3d kSubmapIdPosition(0.0, 0.0, 0.3); +constexpr float kSubmapIdCharHeight = 0.2f; + } // namespace DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, @@ -53,9 +57,14 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, : id_(id), display_context_(display_context), submap_node_(map_node->createChildSceneNode()), + submap_id_text_node_(submap_node_->createChildSceneNode()), ogre_submap_(id, display_context->getSceneManager(), submap_node_), pose_axes_(display_context->getSceneManager(), submap_node_, pose_axes_length, pose_axes_radius), + submap_id_text_(QString("(%1,%2)") + .arg(id.trajectory_id) + .arg(id.submap_index) + .toStdString()), last_query_timestamp_(0) { // DrawableSubmap creates and manages its visibility property object // (a unique_ptr is needed because the Qt parent of the visibility @@ -64,6 +73,13 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); + submap_id_text_.setCharacterHeight(kSubmapIdCharHeight); + 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_); submap_node_->setVisible(visible); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -75,6 +91,7 @@ DrawableSubmap::~DrawableSubmap() { rpc_request_future_.wait(); } display_context_->getSceneManager()->destroySceneNode(submap_node_); + display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_); } void DrawableSubmap::Update( diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 82e8153..fa82020 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -35,6 +35,7 @@ #include "rviz/display_context.h" #include "rviz/frame_manager.h" #include "rviz/ogre_helpers/axes.h" +#include "rviz/ogre_helpers/movable_text.h" #include "rviz/properties/bool_property.h" namespace cartographer_rviz { @@ -91,9 +92,11 @@ class DrawableSubmap : public QObject { ::cartographer::common::Mutex mutex_; ::rviz::DisplayContext* const display_context_; Ogre::SceneNode* const submap_node_; + Ogre::SceneNode* const submap_id_text_node_; OgreSubmap ogre_submap_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::rviz::Axes pose_axes_; + ::rviz::MovableText submap_id_text_; std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); bool query_in_progress_ = false GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_);