From 6f171dc5a78ff4fa714227889eb8a3fd9e1bff87 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Mon, 10 Jul 2017 15:43:11 +0200 Subject: [PATCH] Use SubmapId in cartographer_rviz. (#419) --- .../cartographer_rviz/drawable_submap.cc | 30 +++++++++---------- .../cartographer_rviz/drawable_submap.h | 12 ++++---- .../cartographer_rviz/submaps_display.cc | 26 ++++++++-------- 3 files changed, 31 insertions(+), 37 deletions(-) diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index a722633..fa7569d 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -47,30 +47,28 @@ constexpr double kFadeOutStartDistanceInMeters = 1.; constexpr double kFadeOutDistanceInMeters = 2.; constexpr float kAlphaUpdateThreshold = 0.2f; -std::string GetSubmapIdentifier(const int trajectory_id, - const int submap_index) { - return std::to_string(trajectory_id) + "-" + std::to_string(submap_index); +std::string GetSubmapIdentifier( + const ::cartographer::mapping::SubmapId& submap_id) { + return std::to_string(submap_id.trajectory_id) + "-" + + std::to_string(submap_id.submap_index); } } // namespace -DrawableSubmap::DrawableSubmap(const int trajectory_id, const int submap_index, +DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, Ogre::SceneManager* const scene_manager, ::rviz::Property* submap_category, const bool visible) - : trajectory_id_(trajectory_id), - submap_index_(submap_index), + : id_(id), scene_manager_(scene_manager), scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()), manual_object_(scene_manager->createManualObject( - kManualObjectPrefix + - GetSubmapIdentifier(trajectory_id, submap_index))), + kManualObjectPrefix + GetSubmapIdentifier(id))), last_query_timestamp_(0) { material_ = Ogre::MaterialManager::getSingleton().getByName( kSubmapSourceMaterialName); material_ = - material_->clone(kSubmapMaterialPrefix + - GetSubmapIdentifier(trajectory_id_, submap_index)); + material_->clone(kSubmapMaterialPrefix + GetSubmapIdentifier(id_)); material_->setReceiveShadows(false); material_->getTechnique(0)->setLightingEnabled(false); material_->setCullingMode(Ogre::CULL_NONE); @@ -123,12 +121,12 @@ void DrawableSubmap::Update( UpdateTransform(); } visibility_->setName( - QString("%1.%2").arg(submap_index_).arg(metadata_version_)); + QString("%1.%2").arg(id_.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(id_.trajectory_id) + .arg(id_.submap_index) .arg(metadata_version_)); } @@ -148,8 +146,8 @@ bool DrawableSubmap::MaybeFetchTexture(ros::ServiceClient* const client) { last_query_timestamp_ = now; rpc_request_future_ = std::async(std::launch::async, [this, client]() { ::cartographer_ros_msgs::SubmapQuery srv; - srv.request.trajectory_id = trajectory_id_; - srv.request.submap_index = submap_index_; + srv.request.trajectory_id = id_.trajectory_id; + srv.request.submap_index = id_.submap_index; if (client->call(srv)) { // We emit a signal to update in the right thread, and pass via the // 'response_' member to simplify the signal-slot connection slightly. @@ -230,7 +228,7 @@ void DrawableSubmap::UpdateSceneNode() { texture_.setNull(); } const std::string texture_name = - kSubmapTexturePrefix + GetSubmapIdentifier(trajectory_id_, submap_index_); + kSubmapTexturePrefix + GetSubmapIdentifier(id_); texture_ = Ogre::TextureManager::getSingleton().loadRawData( texture_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, pixel_stream, response_.width, response_.height, Ogre::PF_BYTE_RGB, diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index bbdfde4..a75de3f 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -29,6 +29,7 @@ #include "OgreTexture.h" #include "OgreVector3.h" #include "cartographer/common/mutex.h" +#include "cartographer/mapping/id.h" #include "cartographer_ros_msgs/SubmapEntry.h" #include "cartographer_ros_msgs/SubmapQuery.h" #include "ros/ros.h" @@ -44,9 +45,8 @@ class DrawableSubmap : public QObject { Q_OBJECT public: - // 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, + // The 'scene_manager' is the Ogre scene manager to which to add a node. + DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, Ogre::SceneManager* scene_manager, ::rviz::Property* submap_category, const bool visible); ~DrawableSubmap() override; @@ -70,8 +70,7 @@ 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_; } + ::cartographer::mapping::SubmapId id() const { return id_; } int version() const { return metadata_version_; } bool visibility() const { return visibility_->getBool(); } void set_visibility(const bool visibility) { @@ -91,8 +90,7 @@ class DrawableSubmap : public QObject { void UpdateTransform(); float UpdateAlpha(float target_alpha); - const int trajectory_id_; - const int submap_index_; + const ::cartographer::mapping::SubmapId id_; ::cartographer::common::Mutex mutex_; Ogre::SceneManager* const scene_manager_; diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index 9b23318..be4e65b 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -119,29 +119,27 @@ void SubmapsDisplay::processMessage( using ::cartographer::mapping::SubmapId; std::set listed_submaps; for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { - listed_submaps.insert( - SubmapId{submap_entry.trajectory_id, submap_entry.submap_index}); - const size_t trajectory_id = submap_entry.trajectory_id; - while (trajectory_id >= trajectories_.size()) { + 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(trajectory_id), QVariant(), + QString("Trajectory %1").arg(id.trajectory_id), QVariant(), QString("List of all submaps in Trajectory %1.") - .arg(trajectory_id), + .arg(id.trajectory_id), submaps_category_), std::map>())); } - auto& trajectory_category = trajectories_[trajectory_id].first; - auto& trajectory = trajectories_[trajectory_id].second; - const int submap_index = submap_entry.submap_index; - if (trajectory.count(submap_index) == 0) { + auto& trajectory_category = trajectories_[id.trajectory_id].first; + auto& trajectory = trajectories_[id.trajectory_id].second; + if (trajectory.count(id.submap_index) == 0) { trajectory.emplace( - submap_index, + id.submap_index, ::cartographer::common::make_unique( - trajectory_id, submap_index, context_->getSceneManager(), - trajectory_category.get(), visibility_all_enabled_->getBool())); + id, context_->getSceneManager(), trajectory_category.get(), + visibility_all_enabled_->getBool())); } - trajectory.at(submap_index) + trajectory.at(id.submap_index) ->Update(msg->header, submap_entry, context_->getFrameManager()); } // Remove all submaps not mentioned in the SubmapList.