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_);