Cleanup of the RViz submaps plugin. (#10)

Uses a more concise name to show in RViz.
Moves updating the z fading out of the SceneManagerListener.
(to be removed in a future PR)
Simplifies SubmapsDisplay::processMessage() and
DrawableSubmap::Transform().

cartographer_ros/rviz_plugin_description.xml

@@ -15,11 +15,12 @@
 -->
 
 <library path="lib/libcartographer_rviz_submaps_visualization">
-  <class name="Cartographer Submap visualization"
+  <class name="Submaps"
          type="cartographer_ros::rviz::SubmapsDisplay"
          base_class_type="rviz::Display">
     <description>
-      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
     </description>
   </class>
 </library>

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,
+  frame_manager->transform(kMapFrame, ros::Time(0) /* latest */,
-                            orientation);
+                           transformed_pose_, position, orientation);
   scene_node_->setPosition(position);
   scene_node_->setOrientation(orientation);
 }

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
+  // Each submap is identified by a 'trajectory_id' plus a 'submap_id'.
-  // 'frame_manager' is needed to transform the scene node before rendering
+  // 'scene_manager' is the Ogre scene manager to which to add a node.
-  // onto the offscreen texture. 'scene_manager' is the Ogre scene manager which
-  // contains all submaps.
   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_;

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<DrawableSubmap>(
-              submap_id, trajectory_id, context_->getFrameManager(),
+              submap_id, trajectory_id, context_->getSceneManager()));
-              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,21 +144,29 @@ void SubmapsDisplay::RequestNewSubmaps() {
   }
 }
 
-void SubmapsDisplay::UpdateMapTexture() {
+void SubmapsDisplay::update(const float wall_dt, const float ros_dt) {
-  ::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 */);
+    ::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(context_->getFrameManager());
+    }
   }
 }
 

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<void()> 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 UpdateTransforms();
-  void UpdateMapTexture();
 
   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_;