12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Show submap ID in rviz (#447)

master
Jihoon Lee 2017-07-28 16:40:25 +02:00 committed by Wolfgang Hess
parent ba28491775
commit a40357b61d
2 changed files with 20 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -42,6 +42,10 @@ constexpr double kFadeOutStartDistanceInMeters = 1.;
constexpr double kFadeOutDistanceInMeters = 2.; constexpr double kFadeOutDistanceInMeters = 2.;
constexpr float kAlphaUpdateThreshold = 0.2f; 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 } // namespace
DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
@ -53,9 +57,14 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
: id_(id), : id_(id),
display_context_(display_context), display_context_(display_context),
submap_node_(map_node->createChildSceneNode()), submap_node_(map_node->createChildSceneNode()),
submap_id_text_node_(submap_node_->createChildSceneNode()),
ogre_submap_(id, display_context->getSceneManager(), submap_node_), ogre_submap_(id, display_context->getSceneManager(), submap_node_),
pose_axes_(display_context->getSceneManager(), submap_node_, pose_axes_(display_context->getSceneManager(), submap_node_,
pose_axes_length, pose_axes_radius), 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) { last_query_timestamp_(0) {
// DrawableSubmap creates and manages its visibility property object // DrawableSubmap creates and manages its visibility property object
// (a unique_ptr is needed because the Qt parent of the visibility // (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>( visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>(
"" /* title */, visible, "" /* description */, submap_category, "" /* title */, visible, "" /* description */, submap_category,
SLOT(ToggleVisibility()), this); 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); submap_node_->setVisible(visible);
connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode()));
} }
@ -75,6 +91,7 @@ DrawableSubmap::~DrawableSubmap() {
rpc_request_future_.wait(); rpc_request_future_.wait();
} }
display_context_->getSceneManager()->destroySceneNode(submap_node_); display_context_->getSceneManager()->destroySceneNode(submap_node_);
display_context_->getSceneManager()->destroySceneNode(submap_id_text_node_);
} }
void DrawableSubmap::Update( void DrawableSubmap::Update(

3
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -35,6 +35,7 @@
#include "rviz/display_context.h" #include "rviz/display_context.h"
#include "rviz/frame_manager.h" #include "rviz/frame_manager.h"
#include "rviz/ogre_helpers/axes.h" #include "rviz/ogre_helpers/axes.h"
#include "rviz/ogre_helpers/movable_text.h"
#include "rviz/properties/bool_property.h" #include "rviz/properties/bool_property.h"
namespace cartographer_rviz { namespace cartographer_rviz {
@ -91,9 +92,11 @@ class DrawableSubmap : public QObject {
::cartographer::common::Mutex mutex_; ::cartographer::common::Mutex mutex_;
::rviz::DisplayContext* const display_context_; ::rviz::DisplayContext* const display_context_;
Ogre::SceneNode* const submap_node_; Ogre::SceneNode* const submap_node_;
Ogre::SceneNode* const submap_id_text_node_;
OgreSubmap ogre_submap_; OgreSubmap ogre_submap_;
::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_);
::rviz::Axes pose_axes_; ::rviz::Axes pose_axes_;
::rviz::MovableText submap_id_text_;
std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_); std::chrono::milliseconds last_query_timestamp_ GUARDED_BY(mutex_);
bool query_in_progress_ = false GUARDED_BY(mutex_); bool query_in_progress_ = false GUARDED_BY(mutex_);
int metadata_version_ = -1 GUARDED_BY(mutex_); int metadata_version_ = -1 GUARDED_BY(mutex_);