From 7e272f2e2b631b3def1c65333ee7be2e6e59f1bb Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Fri, 28 Jul 2017 13:46:58 +0200 Subject: [PATCH] Add toggleable trajectories to RViz plugin (#456) Implementation of #422 by @ojura. Original feature idea and implementation by @jihoonl. --- .../cartographer_rviz/submaps_display.cc | 79 +++++++++++-------- .../cartographer_rviz/submaps_display.h | 21 ++++- 2 files changed, 64 insertions(+), 36 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 5eb9657..04ada82 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -53,13 +53,13 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { "Tracking frame", kDefaultTrackingFrame, "Tracking frame, used for fading out submaps.", this); client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(""); - submaps_category_ = new ::rviz::Property( + trajectories_category_ = new ::rviz::Property( "Submaps", QVariant(), "List of all submaps, organized by trajectories.", this); visibility_all_enabled_ = new ::rviz::BoolProperty( "All Enabled", true, - "Whether all the submaps should be displayed or not.", submaps_category_, - SLOT(AllEnabledToggled()), this); + "Whether submaps from all trajectories should be displayed or not.", + trajectories_category_, SLOT(AllEnabledToggled()), this); const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); @@ -111,9 +111,9 @@ void SubmapsDisplay::processMessage( if (trajectory_id >= trajectories_.size()) { continue; } - const auto& trajectory = trajectories_[trajectory_id].second; - const auto it = trajectory.find(submap_entry.submap_index); - if (it != trajectory.end() && + const auto& trajectory_submaps = trajectories_[trajectory_id]->submaps; + const auto it = trajectory_submaps.find(submap_entry.submap_index); + if (it != trajectory_submaps.end() && it->second->version() > submap_entry.submap_version) { // Versions should only increase unless Cartographer restarted. trajectories_.clear(); @@ -126,36 +126,40 @@ 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(Trajectory( - ::cartographer::common::make_unique<::rviz::Property>( - QString("Trajectory %1").arg(id.trajectory_id), QVariant(), - QString("List of all submaps in Trajectory %1.") + trajectories_.push_back(::cartographer::common::make_unique( + ::cartographer::common::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), - submaps_category_), - std::map>())); + trajectories_category_))); } - auto& trajectory_category = trajectories_[id.trajectory_id].first; - auto& trajectory = trajectories_[id.trajectory_id].second; - if (trajectory.count(id.submap_index) == 0) { + auto& trajectory_visibility = trajectories_[id.trajectory_id]->visibility; + auto& trajectory_submaps = trajectories_[id.trajectory_id]->submaps; + if (trajectory_submaps.count(id.submap_index) == 0) { // 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_, map_node_, trajectory_category.get(), - visibility_all_enabled_->getBool(), - kSubmapPoseAxesLength, kSubmapPoseAxesRadius)); + trajectory_submaps.emplace( + id.submap_index, + ::cartographer::common::make_unique( + id, context_, map_node_, trajectory_visibility.get(), + trajectory_visibility->getBool(), kSubmapPoseAxesLength, + kSubmapPoseAxesRadius)); } - trajectory.at(id.submap_index)->Update(msg->header, submap_entry); + trajectory_submaps.at(id.submap_index)->Update(msg->header, submap_entry); } // Remove all submaps not mentioned in the SubmapList. for (size_t trajectory_id = 0; trajectory_id < trajectories_.size(); ++trajectory_id) { - auto& trajectory = trajectories_[trajectory_id].second; - for (auto it = trajectory.begin(); it != trajectory.end();) { + auto& trajectory_submaps = trajectories_[trajectory_id]->submaps; + for (auto it = trajectory_submaps.begin(); + it != trajectory_submaps.end();) { if (listed_submaps.count( SubmapId{static_cast(trajectory_id), it->first}) == 0) { - it = trajectory.erase(it); + it = trajectory_submaps.erase(it); } else { ++it; } @@ -168,13 +172,13 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { // Schedule fetching of new submap textures. for (const auto& trajectory : trajectories_) { int num_ongoing_requests = 0; - for (const auto& submap_entry : trajectory.second) { + for (const auto& submap_entry : trajectory->submaps) { if (submap_entry.second->QueryInProgress()) { ++num_ongoing_requests; } } - for (auto it = trajectory.second.rbegin(); - it != trajectory.second.rend() && + for (auto it = trajectory->submaps.rbegin(); + it != trajectory->submaps.rend() && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; ++it) { if (it->second->MaybeFetchTexture(&client_)) { @@ -192,7 +196,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { tf_buffer_.lookupTransform( *map_frame_, tracking_frame_property_->getStdString(), kLatest); for (auto& trajectory : trajectories_) { - for (auto& submap_entry : trajectory.second) { + for (auto& submap_entry : trajectory->submaps) { submap_entry.second->SetAlpha( transform_stamped.transform.translation.z); } @@ -213,14 +217,25 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { void SubmapsDisplay::AllEnabledToggled() { ::cartographer::common::MutexLocker locker(&mutex_); - const bool visibility = visibility_all_enabled_->getBool(); + const bool visible = visibility_all_enabled_->getBool(); for (auto& trajectory : trajectories_) { - for (auto& submap_entry : trajectory.second) { - submap_entry.second->set_visibility(visibility); - } + trajectory->visibility->setBool(visible); } } +void Trajectory::AllEnabledToggled() { + const bool visible = visibility->getBool(); + for (auto& submap_entry : submaps) { + submap_entry.second->set_visibility(visible); + } +} + +Trajectory::Trajectory(std::unique_ptr<::rviz::BoolProperty> property) + : visibility(std::move(property)) { + ::QObject::connect(visibility.get(), SIGNAL(changed()), this, + SLOT(AllEnabledToggled())); +} + } // namespace cartographer_rviz PLUGINLIB_EXPORT_CLASS(cartographer_rviz::SubmapsDisplay, ::rviz::Display) diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index 9febb08..27a749e 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -32,6 +32,21 @@ namespace cartographer_rviz { +// This should be a private class in SubmapsDisplay, unfortunately, QT does not +// allow for this. +struct Trajectory : public QObject { + Q_OBJECT + + public: + Trajectory(std::unique_ptr<::rviz::BoolProperty> property); + + std::unique_ptr<::rviz::BoolProperty> visibility; + std::map> submaps; + + private Q_SLOTS: + void AllEnabledToggled(); +}; + // RViz plugin used for displaying maps which are represented by a collection of // submaps. // @@ -70,11 +85,9 @@ class SubmapsDisplay std::unique_ptr map_frame_; ::rviz::StringProperty* tracking_frame_property_; Ogre::SceneNode* map_node_ = nullptr; // Represents the map frame. - using Trajectory = std::pair, - std::map>>; - std::vector trajectories_ GUARDED_BY(mutex_); + std::vector> trajectories_ GUARDED_BY(mutex_); ::cartographer::common::Mutex mutex_; - ::rviz::Property* submaps_category_; + ::rviz::Property* trajectories_category_; ::rviz::BoolProperty* visibility_all_enabled_; };