Simplify sensor subscribers. (#434)

cartographer_ros/cartographer_ros/node.cc

@@ -48,33 +48,6 @@ namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
 
-namespace {
-
-void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
-                        int trajectory_id) {
-  if (subscribers.count(trajectory_id) == 0) {
-    return;
-  }
-  subscribers[trajectory_id].shutdown();
-  LOG(INFO) << "Shutdown the subscriber of ["
-            << subscribers[trajectory_id].getTopic() << "]";
-  CHECK_EQ(subscribers.erase(trajectory_id), 1);
-}
-
-bool IsTopicNameUnique(
-    const string& topic,
-    const std::unordered_map<int, ::ros::Subscriber>& subscribers) {
-  for (auto& entry : subscribers) {
-    if (entry.second.getTopic() == topic) {
-      LOG(ERROR) << "Topic name [" << topic << "] is already used.";
-      return false;
-    }
-  }
-  return true;
-}
-
-}  // namespace
-
 Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
     : node_options_(node_options),
       map_builder_bridge_(node_options_, tf_buffer) {
@@ -247,24 +220,20 @@ void Node::SpinOccupancyGridThreadForever() {
   }
 }
 
-int Node::AddTrajectory(const TrajectoryOptions& options,
+std::unordered_set<string> Node::ComputeExpectedTopics(
+    const TrajectoryOptions& options,
     const cartographer_ros_msgs::SensorTopics& topics) {
-  std::unordered_set<string> expected_sensor_ids;
+  std::unordered_set<string> expected_topics;
 
   if (options.use_laser_scan) {
-    expected_sensor_ids.insert(topics.laser_scan_topic);
+    expected_topics.insert(topics.laser_scan_topic);
   }
   if (options.use_multi_echo_laser_scan) {
-    expected_sensor_ids.insert(topics.multi_echo_laser_scan_topic);
+    expected_topics.insert(topics.multi_echo_laser_scan_topic);
-  }
-  if (options.num_point_clouds > 0) {
-    for (int i = 0; i < options.num_point_clouds; ++i) {
-      string topic = topics.point_cloud2_topic;
-      if (options.num_point_clouds > 1) {
-        topic += "_" + std::to_string(i + 1);
-      }
-      expected_sensor_ids.insert(topic);
   }
+  for (const string& topic : ComputeRepeatedTopicNames(
+           topics.point_cloud2_topic, options.num_point_clouds)) {
+    expected_topics.insert(topic);
   }
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.
@@ -272,12 +241,24 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
       (node_options_.map_builder_options.use_trajectory_builder_2d() &&
        options.trajectory_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
-    expected_sensor_ids.insert(topics.imu_topic);
+    expected_topics.insert(topics.imu_topic);
   }
   if (options.use_odometry) {
-    expected_sensor_ids.insert(topics.odometry_topic);
+    expected_topics.insert(topics.odometry_topic);
   }
-  return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+  return expected_topics;
+}
+
+int Node::AddTrajectory(const TrajectoryOptions& options,
+                        const cartographer_ros_msgs::SensorTopics& topics) {
+  const std::unordered_set<string> expected_sensor_ids =
+      ComputeExpectedTopics(options, topics);
+  const int trajectory_id =
+      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+  LaunchSubscribers(options, topics, trajectory_id);
+  subscribed_topics_.insert(expected_sensor_ids.begin(),
+                            expected_sensor_ids.end());
+  return trajectory_id;
 }
 
 void Node::LaunchSubscribers(const TrajectoryOptions& options,
@@ -285,7 +266,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                              const int trajectory_id) {
   if (options.use_laser_scan) {
     const string topic = topics.laser_scan_topic;
-    laser_scan_subscribers_[trajectory_id] =
+    subscribers_[trajectory_id].push_back(
         node_handle_.subscribe<sensor_msgs::LaserScan>(
             topic, kInfiniteSubscriberQueueSize,
             boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
@@ -293,31 +274,25 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                  topic](const sensor_msgs::LaserScan::ConstPtr& msg) {
                   map_builder_bridge_.sensor_bridge(trajectory_id)
                       ->HandleLaserScanMessage(topic, msg);
-                }));
+                })));
   }
 
   if (options.use_multi_echo_laser_scan) {
     const string topic = topics.multi_echo_laser_scan_topic;
-    multi_echo_laser_scan_subscribers_[trajectory_id] =
+    subscribers_[trajectory_id].push_back(node_handle_.subscribe<
-        node_handle_.subscribe<sensor_msgs::MultiEchoLaserScan>(
+                                          sensor_msgs::MultiEchoLaserScan>(
         topic, kInfiniteSubscriberQueueSize,
-            boost::function<void(
+        boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
-                const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
             [this, trajectory_id,
              topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
               map_builder_bridge_.sensor_bridge(trajectory_id)
                   ->HandleMultiEchoLaserScanMessage(topic, msg);
-                }));
+            })));
   }
 
-  std::vector<::ros::Subscriber> grouped_point_cloud_subscribers;
+  for (const string& topic : ComputeRepeatedTopicNames(
-  if (options.num_point_clouds > 0) {
+           topics.point_cloud2_topic, options.num_point_clouds)) {
-    for (int i = 0; i < options.num_point_clouds; ++i) {
+    subscribers_[trajectory_id].push_back(node_handle_.subscribe(
-      string topic = topics.point_cloud2_topic;
-      if (options.num_point_clouds > 1) {
-        topic += "_" + std::to_string(i + 1);
-      }
-      grouped_point_cloud_subscribers.push_back(node_handle_.subscribe(
         topic, kInfiniteSubscriberQueueSize,
         boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
             [this, trajectory_id,
@@ -326,8 +301,6 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                   ->HandlePointCloud2Message(topic, msg);
             })));
   }
-    point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers;
-  }
 
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.
@@ -336,19 +309,20 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
        options.trajectory_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
     string topic = topics.imu_topic;
-    imu_subscribers_[trajectory_id] = node_handle_.subscribe<sensor_msgs::Imu>(
+    subscribers_[trajectory_id].push_back(
+        node_handle_.subscribe<sensor_msgs::Imu>(
             topic, kInfiniteSubscriberQueueSize,
             boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
                 [this, trajectory_id,
                  topic](const sensor_msgs::Imu::ConstPtr& msg) {
                   map_builder_bridge_.sensor_bridge(trajectory_id)
                       ->HandleImuMessage(topic, msg);
-            }));
+                })));
   }
 
   if (options.use_odometry) {
     string topic = topics.odometry_topic;
-    odom_subscribers_[trajectory_id] =
+    subscribers_[trajectory_id].push_back(
         node_handle_.subscribe<nav_msgs::Odometry>(
             topic, kInfiniteSubscriberQueueSize,
             boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
@@ -356,7 +330,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                  topic](const nav_msgs::Odometry::ConstPtr& msg) {
                   map_builder_bridge_.sensor_bridge(trajectory_id)
                       ->HandleOdometryMessage(topic, msg);
-                }));
+                })));
   }
 
   is_active_trajectory_[trajectory_id] = true;
@@ -379,35 +353,14 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
   return false;
 }
 
-bool Node::ValidateTopicName(
+bool Node::ValidateTopicNames(
     const ::cartographer_ros_msgs::SensorTopics& topics,
     const TrajectoryOptions& options) {
-  if (!IsTopicNameUnique(topics.laser_scan_topic, laser_scan_subscribers_)) {
+  for (const std::string& topic : ComputeExpectedTopics(options, topics)) {
+    if (subscribed_topics_.count(topic) > 0) {
+      LOG(ERROR) << "Topic name [" << topic << "] is already used.";
       return false;
     }
-  if (!IsTopicNameUnique(topics.multi_echo_laser_scan_topic,
-                         multi_echo_laser_scan_subscribers_)) {
-    return false;
-  }
-  if (!IsTopicNameUnique(topics.imu_topic, imu_subscribers_)) {
-    return false;
-  }
-  if (!IsTopicNameUnique(topics.odometry_topic, odom_subscribers_)) {
-    return false;
-  }
-  for (auto& subscribers : point_cloud_subscribers_) {
-    string topic = topics.point_cloud2_topic;
-    int count = 0;
-    for (auto& subscriber : subscribers.second) {
-      if (options.num_point_clouds > 1) {
-        topic += "_" + std::to_string(count + 1);
-        ++count;
-      }
-      if (subscriber.getTopic() == topic) {
-        LOG(ERROR) << "Topic name [" << topic << "] is already used";
-        return false;
-      }
-    }
   }
   return true;
 }
@@ -422,13 +375,12 @@ bool Node::HandleStartTrajectory(
     LOG(ERROR) << "Invalid trajectory options.";
     return false;
   }
-  if (!Node::ValidateTopicName(request.topics, options)) {
+  if (!Node::ValidateTopicNames(request.topics, options)) {
     LOG(ERROR) << "Invalid topics.";
     return false;
   }
 
   const int trajectory_id = AddTrajectory(options, request.topics);
-  LaunchSubscribers(options, request.topics, trajectory_id);
   response.trajectory_id = trajectory_id;
 
   is_active_trajectory_[trajectory_id] = true;
@@ -445,7 +397,6 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
   topics.odometry_topic = kOdometryTopic;
 
   const int trajectory_id = AddTrajectory(options, topics);
-  LaunchSubscribers(options, topics, trajectory_id);
   is_active_trajectory_[trajectory_id] = true;
 }
 
@@ -464,18 +415,13 @@ bool Node::HandleFinishTrajectory(
     return false;
   }
 
-  ShutdownSubscriber(laser_scan_subscribers_, trajectory_id);
+  // Shutdown the subscribers of this trajectory.
-  ShutdownSubscriber(multi_echo_laser_scan_subscribers_, trajectory_id);
+  for (auto& entry : subscribers_[trajectory_id]) {
-  ShutdownSubscriber(odom_subscribers_, trajectory_id);
-  ShutdownSubscriber(imu_subscribers_, trajectory_id);
-
-  if (point_cloud_subscribers_.count(trajectory_id) != 0) {
-    for (auto& entry : point_cloud_subscribers_[trajectory_id]) {
-      LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]";
     entry.shutdown();
+    subscribed_topics_.erase(entry.getTopic());
+    LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]";
   }
-    CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1);
+  CHECK_EQ(subscribers_.erase(trajectory_id), 1);
-  }
   map_builder_bridge_.FinishTrajectory(trajectory_id);
   is_active_trajectory_[trajectory_id] = false;
   return true;

cartographer_ros/cartographer_ros/node.h

@@ -18,6 +18,8 @@
 #define CARTOGRAPHER_ROS_NODE_H_
 
 #include <memory>
+#include <unordered_map>
+#include <unordered_set>
 #include <vector>
 
 #include "cartographer/common/mutex.h"
@@ -72,6 +74,10 @@ class Node {
   bool HandleWriteAssets(
       cartographer_ros_msgs::WriteAssets::Request& request,
       cartographer_ros_msgs::WriteAssets::Response& response);
+  // Returns the set of topic names we want to subscribe to.
+  std::unordered_set<string> ComputeExpectedTopics(
+      const TrajectoryOptions& options,
+      const cartographer_ros_msgs::SensorTopics& topics);
   int AddTrajectory(const TrajectoryOptions& options,
                     const cartographer_ros_msgs::SensorTopics& topics);
   void LaunchSubscribers(const TrajectoryOptions& options,
@@ -83,7 +89,7 @@ class Node {
   void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
   bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
-  bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics,
+  bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics,
                           const TrajectoryOptions& options);
 
   const NodeOptions node_options_;
@@ -104,12 +110,8 @@ class Node {
       cartographer::common::Time::min();
 
   // These are keyed with 'trajectory_id'.
-  std::unordered_map<int, ::ros::Subscriber> laser_scan_subscribers_;
+  std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
-  std::unordered_map<int, ::ros::Subscriber> multi_echo_laser_scan_subscribers_;
+  std::unordered_set<std::string> subscribed_topics_;
-  std::unordered_map<int, ::ros::Subscriber> odom_subscribers_;
-  std::unordered_map<int, ::ros::Subscriber> imu_subscribers_;
-  std::unordered_map<int, std::vector<::ros::Subscriber>>
-      point_cloud_subscribers_;
   std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);
   ::ros::Publisher occupancy_grid_publisher_;
   std::thread occupancy_grid_thread_;

cartographer_ros/cartographer_ros/node_constants.cc

@@ -0,0 +1,37 @@
+/*
+ * 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 "cartographer_ros/node_constants.h"
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
+                                                   const int num_topics) {
+  CHECK_GE(num_topics, 0);
+  if (num_topics == 1) {
+    return {topic};
+  }
+  std::vector<std::string> topics;
+  topics.reserve(num_topics);
+  for (int i = 0; i < num_topics; ++i) {
+    topics.emplace_back(topic + "_" + std::to_string(i + 1));
+  }
+  return topics;
+}
+
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node_constants.h

@@ -17,6 +17,9 @@
 #ifndef CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
 #define CARTOGRAPHER_ROS_NODE_CONSTANTS_H_
 
+#include <string>
+#include <vector>
+
 namespace cartographer_ros {
 
 // Default topic names; expected to be remapped as needed.
@@ -39,6 +42,10 @@ constexpr double kConstraintPublishPeriodSec = 0.5;
 constexpr int kInfiniteSubscriberQueueSize = 0;
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 
+// For multiple topics adds numbers to the topic name and returns the list.
+std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
+                                                   int num_topics);
+
 }  // namespace cartographer_ros
 
 #endif  // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -123,17 +123,11 @@ void Run(const std::vector<string>& bag_filenames) {
     check_insert(kMultiEchoLaserScanTopic);
   }
 
-  // For 3D SLAM, subscribe to all point clouds topics.
+  // Subscribe to all point cloud topics.
-  if (trajectory_options.num_point_clouds > 0) {
+  for (const string& topic : ComputeRepeatedTopicNames(
-    for (int i = 0; i < trajectory_options.num_point_clouds; ++i) {
+           kPointCloud2Topic, trajectory_options.num_point_clouds)) {
-      // TODO(hrapp): This code is duplicated in places. Pull out a method.
-      string topic = kPointCloud2Topic;
-      if (trajectory_options.num_point_clouds > 1) {
-        topic += "_" + std::to_string(i + 1);
-      }
     check_insert(topic);
   }
-  }
 
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.