From 2ee9a77a730b105284c8c939c40b969f0c3fbf63 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 28 Jul 2017 12:10:26 +0200 Subject: [PATCH] Fix fixed frame issues of the RViz SubmapsDisplay. (#454) Adds a 'map_node_' which gets updated with the transform to RViz's fixed frame. Before, submaps rendered incorrectly if a fixed frame different from the map frame was chosen. Removes the map frame property since this data is published on the submap list topic. Renames node names to reflect the frames they represent. --- .../configuration_files/demo_2d.rviz | 1 - .../configuration_files/demo_3d.rviz | 1 - .../cartographer_rviz/drawable_submap.cc | 28 ++++----- .../cartographer_rviz/drawable_submap.h | 9 ++- .../cartographer_rviz/ogre_submap.cc | 14 ++--- .../cartographer_rviz/ogre_submap.h | 6 +- .../cartographer_rviz/submaps_display.cc | 60 ++++++++++++------- .../cartographer_rviz/submaps_display.h | 4 +- 8 files changed, 64 insertions(+), 59 deletions(-) diff --git a/cartographer_ros/configuration_files/demo_2d.rviz b/cartographer_ros/configuration_files/demo_2d.rviz index 8a2820b..f09a8d2 100644 --- a/cartographer_ros/configuration_files/demo_2d.rviz +++ b/cartographer_ros/configuration_files/demo_2d.rviz @@ -133,7 +133,6 @@ Visualization Manager: Visual Enabled: true - Class: Submaps Enabled: true - Map frame: map Name: Submaps Submap query service: /submap_query Topic: /submap_list diff --git a/cartographer_ros/configuration_files/demo_3d.rviz b/cartographer_ros/configuration_files/demo_3d.rviz index afab1c8..89e4f42 100644 --- a/cartographer_ros/configuration_files/demo_3d.rviz +++ b/cartographer_ros/configuration_files/demo_3d.rviz @@ -195,7 +195,6 @@ Visualization Manager: Value: false - Class: Submaps Enabled: true - Map frame: map Name: Submaps Submap query service: /submap_query Topic: /submap_list diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 4dcf401..97a8489 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -46,16 +46,15 @@ constexpr float kAlphaUpdateThreshold = 0.2f; 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 float pose_axes_radius) : id_(id), display_context_(display_context), - scene_node_(display_context->getSceneManager() - ->getRootSceneNode() - ->createChildSceneNode()), - ogre_submap_(id, display_context->getSceneManager(), scene_node_), - pose_axes_(display_context->getSceneManager(), scene_node_, + submap_node_(map_node->createChildSceneNode()), + ogre_submap_(id, display_context->getSceneManager(), submap_node_), + pose_axes_(display_context->getSceneManager(), submap_node_, pose_axes_length, pose_axes_radius), last_query_timestamp_(0) { // DrawableSubmap creates and manages its visibility property object @@ -65,7 +64,7 @@ DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, visibility_ = ::cartographer::common::make_unique<::rviz::BoolProperty>( "" /* title */, visible, "" /* description */, submap_category, SLOT(ToggleVisibility()), this); - scene_node_->setVisible(visible); + submap_node_->setVisible(visible); connect(this, SIGNAL(RequestSucceeded()), this, SLOT(UpdateSceneNode())); } @@ -75,24 +74,17 @@ DrawableSubmap::~DrawableSubmap() { if (QueryInProgress()) { rpc_request_future_.wait(); } - display_context_->getSceneManager()->destroySceneNode(scene_node_); + display_context_->getSceneManager()->destroySceneNode(submap_node_); } void DrawableSubmap::Update( const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata, - ::rviz::FrameManager* const frame_manager) { - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!frame_manager->transform(header, metadata.pose, position, orientation)) { - // We don't know where we would display the texture, so we stop here. - return; - } + const ::cartographer_ros_msgs::SubmapEntry& metadata) { ::cartographer::common::MutexLocker locker(&mutex_); metadata_version_ = metadata.submap_version; pose_ = ::cartographer_ros::ToRigid3d(metadata.pose); - scene_node_->setPosition(ToOgre(pose_.translation())); - scene_node_->setOrientation(ToOgre(pose_.rotation())); + submap_node_->setPosition(ToOgre(pose_.translation())); + submap_node_->setOrientation(ToOgre(pose_.rotation())); display_context_->queueRender(); visibility_->setName( QString("%1.%2").arg(id_.submap_index).arg(metadata_version_)); @@ -164,7 +156,7 @@ void DrawableSubmap::UpdateSceneNode() { } void DrawableSubmap::ToggleVisibility() { - scene_node_->setVisible(visibility_->getBool()); + submap_node_->setVisible(visibility_->getBool()); display_context_->queueRender(); } diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.h b/cartographer_rviz/cartographer_rviz/drawable_submap.h index c5a73ed..82e8153 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.h +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.h @@ -47,8 +47,8 @@ class DrawableSubmap : public QObject { public: DrawableSubmap(const ::cartographer::mapping::SubmapId& submap_id, ::rviz::DisplayContext* display_context, - ::rviz::Property* submap_category, bool visible, - float pose_axes_length, float pose_axes_radius); + Ogre::SceneNode* map_node, ::rviz::Property* submap_category, + bool visible, float pose_axes_length, float pose_axes_radius); ~DrawableSubmap() override; DrawableSubmap(const DrawableSubmap&) = delete; DrawableSubmap& operator=(const DrawableSubmap&) = delete; @@ -56,8 +56,7 @@ class DrawableSubmap : public QObject { // Updates the 'metadata' for this submap. If necessary, the next call to // MaybeFetchTexture() will fetch a new submap texture. void Update(const ::std_msgs::Header& header, - const ::cartographer_ros_msgs::SubmapEntry& metadata, - ::rviz::FrameManager* frame_manager); + const ::cartographer_ros_msgs::SubmapEntry& metadata); // If an update is needed, it will send an RPC using 'client' to request the // new data for the submap and returns true. @@ -91,7 +90,7 @@ class DrawableSubmap : public QObject { ::cartographer::common::Mutex mutex_; ::rviz::DisplayContext* const display_context_; - Ogre::SceneNode* const scene_node_; + Ogre::SceneNode* const submap_node_; OgreSubmap ogre_submap_; ::cartographer::transform::Rigid3d pose_ GUARDED_BY(mutex_); ::rviz::Axes pose_axes_; diff --git a/cartographer_rviz/cartographer_rviz/ogre_submap.cc b/cartographer_rviz/cartographer_rviz/ogre_submap.cc index af31faa..2b03fac 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_submap.cc +++ b/cartographer_rviz/cartographer_rviz/ogre_submap.cc @@ -50,11 +50,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id, Ogre::SceneManager* const scene_manager, - Ogre::SceneNode* const scene_node) + Ogre::SceneNode* const submap_node) : id_(id), scene_manager_(scene_manager), - scene_node_(scene_node), - submap_node_(scene_node_->createChildSceneNode()), + submap_node_(submap_node), + slice_node_(submap_node_->createChildSceneNode()), manual_object_(scene_manager_->createManualObject( kManualObjectPrefix + GetSubmapIdentifier(id))) { material_ = Ogre::MaterialManager::getSingleton().getByName( @@ -66,7 +66,7 @@ OgreSubmap::OgreSubmap(const ::cartographer::mapping::SubmapId& id, material_->setCullingMode(Ogre::CULL_NONE); material_->setDepthBias(-1.f, 0.f); material_->setDepthWriteEnabled(false); - submap_node_->attachObject(manual_object_); + slice_node_->attachObject(manual_object_); } OgreSubmap::~OgreSubmap() { @@ -75,14 +75,14 @@ OgreSubmap::~OgreSubmap() { Ogre::TextureManager::getSingleton().remove(texture_->getHandle()); texture_.setNull(); } - scene_manager_->destroySceneNode(submap_node_); + scene_manager_->destroySceneNode(slice_node_); scene_manager_->destroyManualObject(manual_object_); } void OgreSubmap::Update( const ::cartographer_ros::SubmapTexture& submap_texture) { - submap_node_->setPosition(ToOgre(submap_texture.slice_pose.translation())); - submap_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation())); + slice_node_->setPosition(ToOgre(submap_texture.slice_pose.translation())); + slice_node_->setOrientation(ToOgre(submap_texture.slice_pose.rotation())); // The call to Ogre's loadRawData below does not work with an RG texture, // therefore we create an RGB one whose blue channel is always 0. std::vector rgb; diff --git a/cartographer_rviz/cartographer_rviz/ogre_submap.h b/cartographer_rviz/cartographer_rviz/ogre_submap.h index 0b2038e..9771869 100644 --- a/cartographer_rviz/cartographer_rviz/ogre_submap.h +++ b/cartographer_rviz/cartographer_rviz/ogre_submap.h @@ -38,11 +38,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q); // are expected to be called from the Ogre thread. class OgreSubmap { public: - // Attaches a node visualizing the submap 'id' to the 'scene_node' which is + // Attaches a node visualizing the submap 'id' to the 'submap_node' which is // expected to represent the submap frame. OgreSubmap(const ::cartographer::mapping::SubmapId& id, Ogre::SceneManager* const scene_manager, - Ogre::SceneNode* const scene_node); + Ogre::SceneNode* const submap_node); ~OgreSubmap(); OgreSubmap(const OgreSubmap&) = delete; @@ -58,8 +58,8 @@ class OgreSubmap { private: const ::cartographer::mapping::SubmapId id_; Ogre::SceneManager* const scene_manager_; - Ogre::SceneNode* const scene_node_; Ogre::SceneNode* const submap_node_; + Ogre::SceneNode* const slice_node_; Ogre::ManualObject* const manual_object_; Ogre::TexturePtr texture_; Ogre::MaterialPtr material_; diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.cc b/cartographer_rviz/cartographer_rviz/submaps_display.cc index b8214af..5eb9657 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.cc +++ b/cartographer_rviz/cartographer_rviz/submaps_display.cc @@ -49,9 +49,6 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", kDefaultSubmapQueryServiceName, "Submap query service to connect to.", this, SLOT(Reset())); - map_frame_property_ = new ::rviz::StringProperty( - "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.", - this); tracking_frame_property_ = new ::rviz::StringProperty( "Tracking frame", kDefaultTrackingFrame, "Tracking frame, used for fading out submaps.", this); @@ -75,7 +72,11 @@ SubmapsDisplay::SubmapsDisplay() : tf_listener_(tf_buffer_) { Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups(); } -SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); } +SubmapsDisplay::~SubmapsDisplay() { + client_.shutdown(); + trajectories_.clear(); + scene_manager_->destroySceneNode(map_node_); +} void SubmapsDisplay::Reset() { reset(); } @@ -86,6 +87,7 @@ void SubmapsDisplay::CreateClient() { void SubmapsDisplay::onInitialize() { MFDClass::onInitialize(); + map_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode(); CreateClient(); } @@ -100,6 +102,8 @@ void SubmapsDisplay::reset() { void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { ::cartographer::common::MutexLocker locker(&mutex_); + map_frame_ = + ::cartographer::common::make_unique(msg->header.frame_id); // In case Cartographer node is relaunched, destroy trajectories from the // previous instance. for (const ::cartographer_ros_msgs::SubmapEntry& submap_entry : msg->submap) { @@ -138,12 +142,11 @@ void SubmapsDisplay::processMessage( constexpr float kSubmapPoseAxesRadius = 0.06f; trajectory.emplace(id.submap_index, ::cartographer::common::make_unique( - id, context_, trajectory_category.get(), + id, context_, map_node_, trajectory_category.get(), visibility_all_enabled_->getBool(), kSubmapPoseAxesLength, kSubmapPoseAxesRadius)); } - trajectory.at(id.submap_index) - ->Update(msg->header, submap_entry, context_->getFrameManager()); + trajectory.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(); @@ -162,22 +165,6 @@ void SubmapsDisplay::processMessage( void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { ::cartographer::common::MutexLocker locker(&mutex_); - // Update the fading by z distance. - try { - const ::geometry_msgs::TransformStamped transform_stamped = - tf_buffer_.lookupTransform(map_frame_property_->getStdString(), - tracking_frame_property_->getStdString(), - ros::Time(0) /* latest */); - for (auto& trajectory : trajectories_) { - for (auto& submap_entry : trajectory.second) { - submap_entry.second->SetAlpha( - transform_stamped.transform.translation.z); - } - } - } catch (const tf2::TransformException& ex) { - ROS_WARN("Could not compute submap fading: %s", ex.what()); - } - // Schedule fetching of new submap textures. for (const auto& trajectory : trajectories_) { int num_ongoing_requests = 0; @@ -195,6 +182,33 @@ void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { } } } + if (map_frame_ == nullptr) { + return; + } + // Update the fading by z distance. + const ros::Time kLatest(0); + try { + const ::geometry_msgs::TransformStamped transform_stamped = + tf_buffer_.lookupTransform( + *map_frame_, tracking_frame_property_->getStdString(), kLatest); + for (auto& trajectory : trajectories_) { + for (auto& submap_entry : trajectory.second) { + submap_entry.second->SetAlpha( + transform_stamped.transform.translation.z); + } + } + } catch (const tf2::TransformException& ex) { + ROS_WARN("Could not compute submap fading: %s", ex.what()); + } + // Update the map frame to fixed frame transform. + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (context_->getFrameManager()->getTransform(*map_frame_, kLatest, position, + orientation)) { + map_node_->setPosition(position); + map_node_->setOrientation(orientation); + context_->queueRender(); + } } void SubmapsDisplay::AllEnabledToggled() { diff --git a/cartographer_rviz/cartographer_rviz/submaps_display.h b/cartographer_rviz/cartographer_rviz/submaps_display.h index a61cbd0..9febb08 100644 --- a/cartographer_rviz/cartographer_rviz/submaps_display.h +++ b/cartographer_rviz/cartographer_rviz/submaps_display.h @@ -19,6 +19,7 @@ #include #include +#include #include #include "cartographer/common/mutex.h" @@ -66,8 +67,9 @@ class SubmapsDisplay ::tf2_ros::TransformListener tf_listener_; ros::ServiceClient client_; ::rviz::StringProperty* submap_query_service_property_; - ::rviz::StringProperty* map_frame_property_; + 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_);