From 13db11d45cbc592d72d967db4d1dd649fcd1aa52 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 21 Apr 2017 11:58:16 +0200 Subject: [PATCH] Add visibility toggling of individual submaps in cartographer_rviz - Add a variable list object to the submaps plugin, containing visibility checkboxes for individual submaps. Each DrawableSubmap creates and manages its visibility property checkbox object. Submaps are hidden by detaching the Ogre manual object from the Ogre scene node. - Fetch if the submap version number is different, possibly lower (meaning Cartographer was restarted) --- .../cartographer_rviz/drawable_submap.cc | 39 ++++++++++++-- .../cartographer_rviz/drawable_submap.h | 13 ++++- .../cartographer_rviz/submaps_display.cc | 52 ++++++++++++++++--- .../cartographer_rviz/submaps_display.h | 6 ++- 4 files changed, 98 insertions(+), 12 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 96dd59d..a722633 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -25,6 +25,7 @@ #include "Eigen/Geometry" #include "OgreGpuProgramParams.h" #include "OgreImage.h" +#include "cartographer/common/make_unique.h" #include "cartographer/common/port.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "eigen_conversions/eigen_msg.h" @@ -54,7 +55,9 @@ std::string GetSubmapIdentifier(const int trajectory_id, } // namespace DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index, - Ogre::SceneManager* const scene_manager) + Ogre::SceneManager* const scene_manager, + ::rviz::Property* submap_category, + const bool visible) : trajectory_id_(trajectory_id), submap_index_(submap_index), scene_manager_(scene_manager), @@ -73,7 +76,16 @@ DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index, material_->setCullingMode(Ogre::CULL_NONE); material_->setDepthBias(-1.f, 0.f); material_->setDepthWriteEnabled(false); - scene_node_->attachObject(manual_object_); + // DrawableSubmap creates and manages its visibility property object + // (a unique_ptr is needed because the Qt parent of the visibility + // property is the submap_category object - the BoolProperty needs + // to be destroyed along with the DrawableSubmap) + visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( + "" /* title */, visible, "" /* description */, submap_category, + SLOT(ToggleVisibility()), this); + if (visible) { + scene_node_->attachObject(manual_object_); + } connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -110,11 +122,20 @@ void DrawableSubmap::Update( // for this submap. UpdateTransform(); } + visibility_->setName( + QString("%1.%2").arg(submap_index_).arg(metadata_version_)); + visibility_->setDescription( + QString("Toggle visibility of this individual submap.

" + "Trajectory %1, submap %2, submap version %3") + .arg(trajectory_id_) + .arg(submap_index_) + .arg(metadata_version_)); } bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { ::cartographer::common::MutexLocker locker(&mutex_); - const bool newer_version_available = texture_version_ < metadata_version_; + // Received metadata version can also be lower - if we restarted Cartographer + const bool newer_version_available = texture_version_ != metadata_version_; const std::chrono::milliseconds now = std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch()); @@ -244,4 +265,16 @@ float DrawableSubmap::UpdateAlpha(const float target_alpha) { return current_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_); + } + } +} + } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index 3ef89b0..8824028 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -34,6 +34,7 @@ #include "ros/ros.h" #include "rviz/display_context.h" #include "rviz/frame_manager.h" +#include "rviz/properties/bool_property.h" namespace cartographer_rviz { @@ -46,7 +47,8 @@ class DrawableSubmap : public QObject { // Each submap is identified by a 'trajectory_id' plus a 'submap_index'. // 'scene_manager' is the Ogre scene manager to which to add a node. DrawableSubmap(int trajectory_id, int submap_index, - Ogre::SceneManager* scene_manager); + Ogre::SceneManager* scene_manager, + ::rviz::Property* submap_category, const bool visible); ~DrawableSubmap() override; DrawableSubmap(const DrawableSubmap&) = delete; DrawableSubmap& operator=(const DrawableSubmap&) = delete; @@ -68,6 +70,13 @@ class DrawableSubmap : public QObject { // 'current_tracking_z'. void SetAlpha(double current_tracking_z); + int submap_index() const { return submap_index_; } + int trajectory_id() const { return trajectory_id_; } + bool visibility() const { return visibility_->getBool(); } + void set_visibility(const bool visibility) { + visibility_->setBool(visibility); + } + Q_SIGNALS: // RPC request succeeded. void RequestSucceeded(); @@ -75,6 +84,7 @@ class DrawableSubmap : public QObject { private Q_SLOTS: // Callback when an rpc request succeeded. void UpdateSceneNode(); + void ToggleVisibility(); private: void UpdateTransform(); @@ -100,6 +110,7 @@ class DrawableSubmap : public QObject { std::future rpc_request_future_; ::cartographer_ros_msgs::SubmapQuery::Response response_ GUARDED_BY(mutex_); float current_alpha_ = 0.f; + std::unique_ptr<::rviz::BoolProperty> visibility_; }; } // namespace cartographer_rviz diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index f36f9df..338fe8e 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -27,6 +27,7 @@ #include "ros/ros.h" #include "rviz/display_context.h" #include "rviz/frame_manager.h" +#include "rviz/properties/bool_property.h" #include "rviz/properties/string_property.h" namespace cartographer_rviz { @@ -54,6 +55,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( + "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); const std::string package_path = ::ros::package::getPath(ROS_PACKAGE_NAME); Ogre::ResourceGroupManager::getSingleton().addResourceLocation( package_path + kMaterialsDirectory, "FileSystem", ROS_PACKAGE_NAME); @@ -91,20 +99,40 @@ void SubmapsDisplay::reset() { void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { ::cartographer::common::MutexLocker locker(&mutex_); + // In case Cartographer node is relaunched, destroy + // trajectories from the previous instance + if (msg->trajectory.size() < trajectories_.size()) { + trajectories_.clear(); + } for (size_t trajectory_id = 0; trajectory_id < msg->trajectory.size(); ++trajectory_id) { if (trajectory_id >= trajectories_.size()) { - trajectories_.emplace_back(); + // When a trajectory is destroyed, it also needs to delete its rviz + // Property object, so we use a unique_ptr for it + trajectories_.push_back(Trajectory( + ::cartographer::common::make_unique<::rviz::Property>( + QString("Trajectory %1").arg(trajectory_id), QVariant(), + QString("List of all submaps in Trajectory %1.") + .arg(trajectory_id), + submaps_category_), + std::vector>())); } - auto& trajectory = trajectories_[trajectory_id]; + auto& trajectory_category = trajectories_[trajectory_id].first; + auto& trajectory = trajectories_[trajectory_id].second; const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = msg->trajectory[trajectory_id].submap; + // Same as above, destroy the whole trajectory if we detect that + // we have more submaps than we should + if (submap_entries.size() < trajectory.size()) { + trajectory.clear(); + } for (size_t submap_index = 0; submap_index < submap_entries.size(); ++submap_index) { if (submap_index >= trajectory.size()) { trajectory.push_back( ::cartographer::common::make_unique( - trajectory_id, submap_index, context_->getSceneManager())); + trajectory_id, submap_index, context_->getSceneManager(), + trajectory_category.get(), visibility_all_enabled_->getBool())); } trajectory[submap_index]->Update(msg->header, submap_entries[submap_index], @@ -122,7 +150,7 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { tracking_frame_property_->getStdString(), ros::Time(0) /* latest */); for (auto& trajectory : trajectories_) { - for (auto& submap : trajectory) { + for (auto& submap : trajectory.second) { submap->SetAlpha(transform_stamped.transform.translation.z); } } @@ -133,22 +161,32 @@ 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 : trajectory) { + for (const auto& submap : trajectory.second) { if (submap->QueryInProgress()) { ++num_ongoing_requests; } } - for (int submap_index = static_cast(trajectory.size()) - 1; + for (int submap_index = static_cast(trajectory.second.size()) - 1; submap_index >= 0 && num_ongoing_requests < kMaxOnGoingRequestsPerTrajectory; --submap_index) { - if (trajectory[submap_index]->MaybeFetchTexture(&client_)) { + if (trajectory.second[submap_index]->MaybeFetchTexture(&client_)) { ++num_ongoing_requests; } } } } +void SubmapsDisplay::AllEnabledToggled() { + ::cartographer::common::MutexLocker locker(&mutex_); + const bool visibility = visibility_all_enabled_->getBool(); + for (auto& trajectory : trajectories_) { + for (auto& submap : trajectory.second) { + submap->set_visibility(visibility); + } + } +} + } // 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 eda4a53..83e5238 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -49,6 +49,7 @@ class SubmapsDisplay private Q_SLOTS: void Reset(); + void AllEnabledToggled(); private: void CreateClient(); @@ -66,9 +67,12 @@ class SubmapsDisplay ::rviz::StringProperty* submap_query_service_property_; ::rviz::StringProperty* map_frame_property_; ::rviz::StringProperty* tracking_frame_property_; - using Trajectory = std::vector>; + using Trajectory = std::pair, + std::vector>>; std::vector trajectories_ GUARDED_BY(mutex_); ::cartographer::common::Mutex mutex_; + ::rviz::Property* submaps_category_; + ::rviz::BoolProperty* visibility_all_enabled_; }; } // namespace cartographer_rviz