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

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

master
Michael Grupp 2018-09-13 13:32:50 +02:00 committed by gaschler
parent c418ff5580
commit aa8a62e82f
4 changed files with 59 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cartographer_rviz/cartographer_rviz/drawable_submap.cc
View File

@ -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

9
cartographer_rviz/cartographer_rviz/drawable_submap.h
View File

@ -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;

42
cartographer_rviz/cartographer_rviz/submaps_display.cc
View File

@ -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(
absl::make_unique<Trajectory>(absl::make_unique<::rviz::BoolProperty>(
trajectories_.push_back(absl::make_unique<Trajectory>(
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

7
cartographer_rviz/cartographer_rviz/submaps_display.h
View File

@ -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_;
};