Change the RViz plugin to use MessageFilterDisplay. (#6)

cartographer_ros/CMakeLists.txt

@@ -100,8 +100,6 @@ add_library(cartographer_rviz_submaps_visualization
   src/node_constants.h
   src/submaps_display.cc
   src/submaps_display.h
-  src/trajectory.cc
-  src/trajectory.h
 )
 target_link_libraries(cartographer_rviz_submaps_visualization
   ${Boost_LIBRARIES}

cartographer_ros/src/cartographer_node_main.cc

@@ -496,7 +496,7 @@ bool Node::HandleSubmapQuery(
   return true;
 }
 
-void Node::PublishSubmapList(int64 timestamp) {
+void Node::PublishSubmapList(const int64 timestamp) {
   ::cartographer::common::MutexLocker lock(&mutex_);
   const ::cartographer::mapping::Submaps* submaps =
       trajectory_builder_->submaps();
@@ -513,12 +513,15 @@ void Node::PublishSubmapList(int64 timestamp) {
   }
 
   ::cartographer_ros_msgs::SubmapList ros_submap_list;
+  ros_submap_list.header.stamp =
+      ToRos(::cartographer::common::FromUniversal(timestamp));
+  ros_submap_list.header.frame_id = map_frame_;
   ros_submap_list.trajectory.push_back(ros_trajectory);
   submap_list_publisher_.publish(ros_submap_list);
   last_submap_list_publish_timestamp_ = timestamp;
 }
 
-void Node::PublishPose(int64 timestamp) {
+void Node::PublishPose(const int64 timestamp) {
   const ::cartographer::common::Time time =
       ::cartographer::common::FromUniversal(timestamp);
   const Rigid3d tracking_to_map = trajectory_builder_->pose_estimate().pose;

cartographer_ros/src/submaps_display.cc

@@ -18,6 +18,7 @@
 
 #include <OgreResourceGroupManager.h>
 #include <OgreSceneManager.h>
+#include <cartographer/common/make_unique.h>
 #include <cartographer/common/mutex.h>
 #include <cartographer_ros_msgs/SubmapList.h>
 #include <cartographer_ros_msgs/SubmapQuery.h>
@@ -47,21 +48,13 @@ constexpr char kDefaultTrackingFrame[] = "base_link";
 }  // namespace
 
 SubmapsDisplay::SubmapsDisplay()
-    : Display(),
+    : scene_manager_listener_([this]() { UpdateMapTexture(); }),
-      scene_manager_listener_([this]() { UpdateMapTexture(); }),
       tf_listener_(tf_buffer_) {
   connect(this, SIGNAL(SubmapListUpdated()), this, SLOT(RequestNewSubmaps()));
-  topic_property_ = new ::rviz::RosTopicProperty(
-      "Topic", QString("/cartographer/") + kSubmapListTopic,
-      QString::fromStdString(
-          ros::message_traits::datatype<::cartographer_ros_msgs::SubmapList>()),
-      "cartographer_ros_msgs::SubmapList topic to subscribe to.", this,
-      SLOT(UpdateSubmapTopicOrService()));
   submap_query_service_property_ = new ::rviz::StringProperty(
       "Submap query service",
       QString("/cartographer/") + kSubmapQueryServiceName,
-      "Submap query service to connect to.", this,
+      "Submap query service to connect to.", this, SLOT(reset()));
-      SLOT(UpdateSubmapTopicOrService()));
   map_frame_property_ = new ::rviz::StringProperty(
       "Map frame", kDefaultMapFrame, "Map frame, used for fading out submaps.",
       this);
@@ -81,60 +74,31 @@ SubmapsDisplay::SubmapsDisplay()
   Ogre::ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
 }
 
-SubmapsDisplay::~SubmapsDisplay() { UnsubscribeAndClear(); }
+SubmapsDisplay::~SubmapsDisplay() { client_.shutdown(); }
 
 void SubmapsDisplay::onInitialize() {
+  MFDClass::onInitialize();
   scene_manager_->addListener(&scene_manager_listener_);
-  UpdateSubmapTopicOrService();
+  CreateClient();
-}
-
-void SubmapsDisplay::UpdateSubmapTopicOrService() {
-  UnsubscribeAndClear();
-  Subscribe();
 }
 
 void SubmapsDisplay::reset() {
-  Display::reset();
+  MFDClass::reset();
-  UpdateSubmapTopicOrService();
+  ::cartographer::common::MutexLocker locker(&mutex_);
+  client_.shutdown();
+  trajectories_.clear();
+  CreateClient();
 }
 
-void SubmapsDisplay::onEnable() { Subscribe(); }
+void SubmapsDisplay::processMessage(
-
-void SubmapsDisplay::onDisable() { UnsubscribeAndClear(); }
-
-void SubmapsDisplay::Subscribe() {
-  if (!isEnabled()) {
-    return;
-  }
-
-  client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
-      submap_query_service_property_->getStdString());
-
-  if (!topic_property_->getTopic().isEmpty()) {
-    try {
-      submap_list_subscriber_ =
-          update_nh_.subscribe(topic_property_->getTopicStd(), 1,
-                               &SubmapsDisplay::IncomingSubmapList, this,
-                               ros::TransportHints().reliable());
-      setStatus(::rviz::StatusProperty::Ok, "Topic", "OK");
-    } catch (const ros::Exception& ex) {
-      setStatus(::rviz::StatusProperty::Error, "Topic",
-                QString("Error subscribing: ") + ex.what());
-    }
-  }
-}
-
-void SubmapsDisplay::IncomingSubmapList(
     const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
   submap_list_ = *msg;
   Q_EMIT SubmapListUpdated();
 }
 
-void SubmapsDisplay::UnsubscribeAndClear() {
+void SubmapsDisplay::CreateClient() {
-  submap_list_subscriber_.shutdown();
+  client_ = update_nh_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
-  client_.shutdown();
+      submap_query_service_property_->getStdString());
-  ::cartographer::common::MutexLocker locker(&mutex_);
-  trajectories_.clear();
 }
 
 void SubmapsDisplay::RequestNewSubmaps() {
@@ -142,24 +106,25 @@ void SubmapsDisplay::RequestNewSubmaps() {
   for (int trajectory_id = 0; trajectory_id < submap_list_.trajectory.size();
        ++trajectory_id) {
     if (trajectory_id >= trajectories_.size()) {
-      trajectories_.emplace_back(new Trajectory);
+      trajectories_.emplace_back();
     }
     const std::vector<::cartographer_ros_msgs::SubmapEntry>& submap_entries =
         submap_list_.trajectory[trajectory_id].submap;
     if (submap_entries.empty()) {
       return;
     }
-    for (int submap_id = trajectories_[trajectory_id]->Size();
+    for (int submap_id = trajectories_[trajectory_id].size();
          submap_id < submap_entries.size(); ++submap_id) {
-      trajectories_[trajectory_id]->Add(submap_id, trajectory_id,
+      trajectories_[trajectory_id].push_back(
-                                        context_->getFrameManager(),
+          ::cartographer::common::make_unique<DrawableSubmap>(
-                                        context_->getSceneManager());
+              submap_id, trajectory_id, context_->getFrameManager(),
+              context_->getSceneManager()));
     }
   }
   int num_ongoing_requests = 0;
   for (const auto& trajectory : trajectories_) {
-    for (int i = 0; i < trajectory->Size(); ++i) {
+    for (const auto& submap : trajectory) {
-      if (trajectory->Get(i).QueryInProgress()) {
+      if (submap->QueryInProgress()) {
         ++num_ongoing_requests;
         if (num_ongoing_requests == kMaxOnGoingRequests) {
           return;
@@ -173,7 +138,7 @@ void SubmapsDisplay::RequestNewSubmaps() {
         submap_list_.trajectory[trajectory_id].submap;
     for (int submap_id = submap_entries.size() - 1; submap_id >= 0;
          --submap_id) {
-      if (trajectories_[trajectory_id]->Get(submap_id).Update(
+      if (trajectories_[trajectory_id][submap_id]->Update(
               submap_entries[submap_id], &client_)) {
         ++num_ongoing_requests;
         if (num_ongoing_requests == kMaxOnGoingRequests) {
@@ -187,15 +152,14 @@ void SubmapsDisplay::RequestNewSubmaps() {
 void SubmapsDisplay::UpdateMapTexture() {
   ::cartographer::common::MutexLocker locker(&mutex_);
   for (auto& trajectory : trajectories_) {
-    for (int i = 0; i < trajectory->Size(); ++i) {
+    for (auto& submap : trajectory) {
-      trajectory->Get(i).Transform(ros::Time(0) /* latest */);
+      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 */);
-        trajectory->Get(i).SetAlpha(
+        submap->SetAlpha(transform_stamped.transform.translation.z);
-            transform_stamped.transform.translation.z);
       } catch (const tf2::TransformException& ex) {
         ROS_WARN("Could not compute submap fading: %s", ex.what());
       }

cartographer_ros/src/submaps_display.h

@@ -27,8 +27,7 @@
 #include <cartographer_ros_msgs/SubmapList.h>
 #include <nav_msgs/MapMetaData.h>
 #include <ros/time.h>
-#include <rviz/display.h>
+#include <rviz/message_filter_display.h>
-#include <rviz/properties/ros_topic_property.h>
 #include <tf/tfMessage.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -36,7 +35,6 @@
 #include <vector>
 
 #include "drawable_submap.h"
-#include "trajectory.h"
 
 namespace cartographer_ros {
 namespace rviz {
@@ -47,7 +45,8 @@ namespace rviz {
 // We show an X-ray view of the map which is achieved by shipping textures for
 // every submap containing pre-multiplied alpha and grayscale values, these are
 // then alpha blended together.
-class SubmapsDisplay : public ::rviz::Display {
+class SubmapsDisplay
+    : public ::rviz::MessageFilterDisplay<::cartographer_ros_msgs::SubmapList> {
   Q_OBJECT
 
  public:
@@ -57,17 +56,11 @@ class SubmapsDisplay : public ::rviz::Display {
   SubmapsDisplay(const SubmapsDisplay&) = delete;
   SubmapsDisplay& operator=(const SubmapsDisplay&) = delete;
 
-  // Called by RViz on initialization of the plugin.
-  void onInitialize() override;
-  // Called to tell the display to clear its state.
-  void reset() override;
-
  Q_SIGNALS:
   void SubmapListUpdated();
 
  private Q_SLOTS:
   void RequestNewSubmaps();
-  void UpdateSubmapTopicOrService();
 
  private:
   class SceneManagerListener : public Ogre::SceneManager::Listener {
@@ -82,13 +75,13 @@ class SubmapsDisplay : public ::rviz::Display {
     std::function<void()> callback_;
   };
 
-  void onEnable() override;
+  void onInitialize() override;
-  void onDisable() override;
+  void reset() override;
-  void Subscribe();
+  void processMessage(
-  void UnsubscribeAndClear();
+      const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg) override;
+
+  void CreateClient();
   void UpdateMapTexture();
-  void IncomingSubmapList(
-      const ::cartographer_ros_msgs::SubmapList::ConstPtr& msg);
 
   SceneManagerListener scene_manager_listener_;
   ::cartographer_ros_msgs::SubmapList submap_list_;
@@ -96,11 +89,11 @@ class SubmapsDisplay : public ::rviz::Display {
   ::tf2_ros::Buffer tf_buffer_;
   ::tf2_ros::TransformListener tf_listener_;
   ros::ServiceClient client_;
-  ::rviz::RosTopicProperty* topic_property_;
   ::rviz::StringProperty* submap_query_service_property_;
   ::rviz::StringProperty* map_frame_property_;
   ::rviz::StringProperty* tracking_frame_property_;
-  std::vector<std::unique_ptr<Trajectory>> trajectories_ GUARDED_BY(mutex_);
+  using Trajectory = std::vector<std::unique_ptr<DrawableSubmap>>;
+  std::vector<Trajectory> trajectories_ GUARDED_BY(mutex_);
   ::cartographer::common::Mutex mutex_;
 };
 

cartographer_ros/src/trajectory.cc

@@ -1,50 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#include "trajectory.h"
-
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <cartographer/common/make_unique.h>
-#include <rviz/frame_manager.h>
-
-#include <memory>
-#include <mutex>
-#include <vector>
-
-#include "drawable_submap.h"
-
-namespace cartographer_ros {
-namespace rviz {
-
-void Trajectory::Add(const int submap_id, const int trajectory_id,
-                     ::rviz::FrameManager* const frame_manager,
-                     Ogre::SceneManager* const scene_manager) {
-  std::lock_guard<std::mutex> guard(mutex_);
-  drawable_submaps_.push_back(
-      ::cartographer::common::make_unique<DrawableSubmap>(
-          submap_id, trajectory_id, frame_manager, scene_manager));
-}
-
-DrawableSubmap& Trajectory::Get(const int index) {
-  std::lock_guard<std::mutex> guard(mutex_);
-  return *drawable_submaps_[index];
-}
-
-int Trajectory::Size() { return drawable_submaps_.size(); }
-
-}  // namespace rviz
-}  // namespace cartographer_ros

cartographer_ros/src/trajectory.h

@@ -1,58 +0,0 @@
-/*
- * Copyright 2016 The Cartographer Authors
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *      http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
-#define CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_
-
-#include <OgreSceneManager.h>
-#include <OgreSceneNode.h>
-#include <rviz/frame_manager.h>
-
-#include <memory>
-#include <mutex>
-#include <vector>
-
-#include "drawable_submap.h"
-
-namespace cartographer_ros {
-namespace rviz {
-
-// Contains a list of drawable submaps.
-class Trajectory {
- public:
-  Trajectory() = default;
-
-  Trajectory(const Trajectory&) = delete;
-  Trajectory& operator=(const Trajectory&) = delete;
-
-  // Creates a new DrawableSubmap and stores it as part of this trajectory.
-  void Add(int submap_id, int trajectory_id,
-           ::rviz::FrameManager* frame_manager,
-           Ogre::SceneManager* scene_manager);
-  // Gets the submap at 'index' and returns a non-const reference.
-  DrawableSubmap& Get(int index);
-  // Returns the number of DrawableSubmaps this trajectory contains.
-  int Size();
-
- private:
-  std::mutex mutex_;
-  std::vector<std::unique_ptr<DrawableSubmap>> drawable_submaps_;
-};
-
-}  // namespace rviz
-}  // namespace cartographer_ros
-
-#endif  // CARTOGRAPHER_ROS_GOOGLE_CARTOGRAPHER_SRC_TRAJECTORY_H_

cartographer_ros_msgs/msg/SubmapList.msg

@@ -12,4 +12,5 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+std_msgs/Header header
 TrajectorySubmapList[] trajectory