diff --git a/cartographer_ros/rviz_plugin_description.xml b/cartographer_ros/rviz_plugin_description.xml index b990bd7..f5c177c 100644 --- a/cartographer_ros/rviz_plugin_description.xml +++ b/cartographer_ros/rviz_plugin_description.xml @@ -15,11 +15,12 @@ --> - - Displays Google Cartographer's submaps as a unified map in RViz. + Displays submaps as a unified map in RViz. + https://github.com/googlecartographer/cartographer_ros diff --git a/cartographer_ros/src/drawable_submap.cc b/cartographer_ros/src/drawable_submap.cc index 7fea94c..61f75d7 100644 --- a/cartographer_ros/src/drawable_submap.cc +++ b/cartographer_ros/src/drawable_submap.cc @@ -57,10 +57,8 @@ std::string GetSubmapIdentifier(const int trajectory_id, const int submap_id) { } // namespace DrawableSubmap::DrawableSubmap(const int submap_id, const int trajectory_id, - ::rviz::FrameManager* const frame_manager, Ogre::SceneManager* const scene_manager) - : frame_manager_(frame_manager), - scene_manager_(scene_manager), + : scene_manager_(scene_manager), scene_node_(scene_manager->getRootSceneNode()->createChildSceneNode()), manual_object_(scene_manager->createManualObject( kManualObjectPrefix + GetSubmapIdentifier(trajectory_id, submap_id))), @@ -240,11 +238,11 @@ void DrawableSubmap::UpdateSceneNode() { texture_unit->setTextureFiltering(Ogre::TFO_NONE); } -void DrawableSubmap::Transform(const ros::Time& ros_time) { +void DrawableSubmap::Transform(::rviz::FrameManager* const frame_manager) { Ogre::Vector3 position; Ogre::Quaternion orientation; - frame_manager_->transform(kMapFrame, ros_time, transformed_pose_, position, - orientation); + frame_manager->transform(kMapFrame, ros::Time(0) /* latest */, + transformed_pose_, position, orientation); scene_node_->setPosition(position); scene_node_->setOrientation(orientation); } diff --git a/cartographer_ros/src/drawable_submap.h b/cartographer_ros/src/drawable_submap.h index a05f5a9..1ebf77d 100644 --- a/cartographer_ros/src/drawable_submap.h +++ b/cartographer_ros/src/drawable_submap.h @@ -41,12 +41,9 @@ class DrawableSubmap : public QObject { Q_OBJECT public: - // Each submap is identified by a 'trajectory_id' plus a 'submap_id'. The - // 'frame_manager' is needed to transform the scene node before rendering - // onto the offscreen texture. 'scene_manager' is the Ogre scene manager which - // contains all submaps. + // Each submap is identified by a 'trajectory_id' plus a 'submap_id'. + // 'scene_manager' is the Ogre scene manager to which to add a node. DrawableSubmap(int submap_id, int trajectory_id, - ::rviz::FrameManager* frame_manager, Ogre::SceneManager* scene_manager); ~DrawableSubmap() override; DrawableSubmap(const DrawableSubmap&) = delete; @@ -63,7 +60,7 @@ class DrawableSubmap : public QObject { // Transforms the scene node for this submap before being rendered onto a // texture. - void Transform(const ros::Time& ros_time); + void Transform(::rviz::FrameManager* frame_manager); // Sets the alpha of the submap taking into account its slice height and the // 'current_tracking_z'. @@ -88,7 +85,6 @@ class DrawableSubmap : public QObject { const int trajectory_id_; ::cartographer::common::Mutex mutex_; - ::rviz::FrameManager* frame_manager_; Ogre::SceneManager* const scene_manager_; Ogre::SceneNode* const scene_node_; Ogre::ManualObject* manual_object_; diff --git a/cartographer_ros/src/submaps_display.cc b/cartographer_ros/src/submaps_display.cc index 4a7d1f8..b8653bb 100644 --- a/cartographer_ros/src/submaps_display.cc +++ b/cartographer_ros/src/submaps_display.cc @@ -48,9 +48,8 @@ constexpr char kDefaultTrackingFrame[] = "base_link"; } // namespace SubmapsDisplay::SubmapsDisplay() - : scene_manager_listener_([this]() { UpdateMapTexture(); }), + : scene_manager_listener_([this]() { UpdateTransforms(); }), tf_listener_(tf_buffer_) { - connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps())); submap_query_service_property_ = new ::rviz::StringProperty( "Submap query service", QString("/cartographer/") + kSubmapQueryServiceName, @@ -76,6 +75,13 @@ SubmapsDisplay::SubmapsDisplay() SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); } +void SubmapsDisplay::Reset() { reset(); } + +void SubmapsDisplay::CreateClient() { + client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( + submap_query_service_property_->getStdString()); +} + void SubmapsDisplay::onInitialize() { MFDClass::onInitialize(); scene_manager_->addListener(&scene_manager_listener_); @@ -92,26 +98,14 @@ void SubmapsDisplay::reset() { void SubmapsDisplay::processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) { - submap_list_ = *msg; - Q_EMIT SubmapListUpdated(); -} - -void SubmapsDisplay::CreateClient() { - client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>( - submap_query_service_property_->getStdString()); -} - -void SubmapsDisplay::Reset() { reset(); } - -void SubmapsDisplay::RequestNewSubmaps() { ::cartographer::common::MutexLocker locker(&mutex_); - for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); + for (int trajectory_id = 0; trajectory_id < msg->trajectory.size(); ++trajectory_id) { if (trajectory_id >= trajectories_.size()) { trajectories_.emplace_back(); } const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = - submap_list_.trajectory[trajectory_id].submap; + msg->trajectory[trajectory_id].submap; if (submap_entries.empty()) { return; } @@ -119,8 +113,7 @@ void SubmapsDisplay::RequestNewSubmaps() { submap_id < submap_entries.size(); ++submap_id) { trajectories_[trajectory_id].push_back( ::cartographer::common::make_unique( - submap_id, trajectory_id, context_->getFrameManager(), - context_->getSceneManager())); + submap_id, trajectory_id, context_->getSceneManager())); } } int num_ongoing_requests = 0; @@ -134,10 +127,10 @@ void SubmapsDisplay::RequestNewSubmaps() { } } } - for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size(); + for (int trajectory_id = 0; trajectory_id < msg->trajectory.size(); ++trajectory_id) { const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries = - submap_list_.trajectory[trajectory_id].submap; + msg->trajectory[trajectory_id].submap; for (int submap_id = submap_entries.size() - 1; submap_id >= 0; --submap_id) { if (trajectories_[trajectory_id][submap_id]->Update( @@ -151,20 +144,28 @@ void SubmapsDisplay::RequestNewSubmaps() { } } -void SubmapsDisplay::UpdateMapTexture() { +void SubmapsDisplay::update(const float wall_dt, const float ros_dt) { + try { + const ::geometry_msgs::TransformStamped transform_stamped = + tf_buffer_.lookupTransform(map_frame_property_->getStdString(), + tracking_frame_property_->getStdString(), + ros::Time(0) /* latest */); + ::cartographer::common::MutexLocker locker(&mutex_); + for (auto& trajectory : trajectories_) { + for (auto& submap : trajectory) { + submap->SetAlpha(transform_stamped.transform.translation.z); + } + } + } catch (const tf2::TransformException& ex) { + ROS_WARN("Could not compute submap fading: %s", ex.what()); + } +} + +void SubmapsDisplay::UpdateTransforms() { ::cartographer::common::MutexLocker locker(&mutex_); for (auto& trajectory : trajectories_) { for (auto& submap : trajectory) { - submap->Transform(ros::Time(0) /* latest */); - try { - const ::geometry_msgs::TransformStamped transform_stamped = - tf_buffer_.lookupTransform(map_frame_property_->getStdString(), - tracking_frame_property_->getStdString(), - ros::Time(0) /* latest */); - submap->SetAlpha(transform_stamped.transform.translation.z); - } catch (const tf2::TransformException& ex) { - ROS_WARN("Could not compute submap fading: %s", ex.what()); - } + submap->Transform(context_->getFrameManager()); } } } diff --git a/cartographer_ros/src/submaps_display.h b/cartographer_ros/src/submaps_display.h index f0b032c..c7fb1bc 100644 --- a/cartographer_ros/src/submaps_display.h +++ b/cartographer_ros/src/submaps_display.h @@ -56,12 +56,8 @@ class SubmapsDisplay SubmapsDisplay(const SubmapsDisplay&) = delete; SubmapsDisplay& operator=(const SubmapsDisplay&) = delete; - Q_SIGNALS: - void SubmapListUpdated(); - private Q_SLOTS: void Reset(); - void RequestNewSubmaps(); private: class SceneManagerListener : public Ogre::SceneManager::Listener { @@ -76,17 +72,17 @@ class SubmapsDisplay std::function callback_; }; + void CreateClient(); + void onInitialize() override; void reset() override; void processMessage( const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override; + void update(float wall_dt, float ros_dt) override; - void CreateClient(); - void UpdateMapTexture(); + void UpdateTransforms(); SceneManagerListener scene_manager_listener_; - ::cartographer_ros_msgs::SubmapList submap_list_; - ros::Subscriber submap_list_subscriber_; ::tf2_ros::Buffer tf_buffer_; ::tf2_ros::TransformListener tf_listener_; ros::ServiceClient client_;