Follow googlecartographer/cartographer#839 (#686)

Follow change googlecartographer/cartographer#839 from string to struct SensorId.
Compute expected sensor ids for multiple trajectories.
Remove command argument input for sensor ids.
Make some methods const.
Clean up.

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -79,7 +79,8 @@ void MapBuilderBridge::LoadMap(const std::string& map_filename) {
 }
 
 int MapBuilderBridge::AddTrajectory(
-    const std::unordered_set<std::string>& expected_sensor_ids,
+    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+        expected_sensor_ids,
     const TrajectoryOptions& trajectory_options) {
   const int trajectory_id = map_builder_->AddTrajectoryBuilder(
       expected_sensor_ids, trajectory_options.trajectory_builder_options,

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -18,13 +18,14 @@
 #define CARTOGRAPHER_ROS_MAP_BUILDER_BRIDGE_H_
 
 #include <memory>
+#include <set>
 #include <string>
 #include <unordered_map>
-#include <unordered_set>
 
 #include "cartographer/common/mutex.h"
 #include "cartographer/mapping/map_builder_interface.h"
 #include "cartographer/mapping/proto/trajectory_builder_options.pb.h"
+#include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
@@ -63,7 +64,10 @@ class MapBuilderBridge {
   MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
 
   void LoadMap(const std::string& map_filename);
-  int AddTrajectory(const std::unordered_set<std::string>& expected_sensor_ids,
+  int AddTrajectory(
+      const std::set<
+          ::cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+          expected_sensor_ids,
       const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
   void RunFinalOptimization();

cartographer_ros/cartographer_ros/node.cc

@@ -255,23 +255,26 @@ void Node::PublishConstraintList(
   }
 }
 
-std::unordered_set<std::string> Node::ComputeExpectedTopics(
+std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+Node::ComputeExpectedSensorIds(
     const TrajectoryOptions& options,
-    const cartographer_ros_msgs::SensorTopics& topics) {
+    const cartographer_ros_msgs::SensorTopics& topics) const {
-  std::unordered_set<std::string> expected_topics;
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+  using SensorType = SensorId::SensorType;
+  std::set<SensorId> expected_topics;
   // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
   for (const std::string& topic : ComputeRepeatedTopicNames(
            topics.laser_scan_topic, options.num_laser_scans)) {
-    expected_topics.insert(topic);
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   for (const std::string& topic :
        ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
                                  options.num_multi_echo_laser_scans)) {
-    expected_topics.insert(topic);
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   for (const std::string& topic : ComputeRepeatedTopicNames(
            topics.point_cloud2_topic, options.num_point_clouds)) {
-    expected_topics.insert(topic);
+    expected_topics.insert(SensorId{SensorType::RANGE, topic});
   }
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.
@@ -279,15 +282,17 @@ std::unordered_set<std::string> Node::ComputeExpectedTopics(
       (node_options_.map_builder_options.use_trajectory_builder_2d() &&
        options.trajectory_builder_options.trajectory_builder_2d_options()
            .use_imu_data())) {
-    expected_topics.insert(topics.imu_topic);
+    expected_topics.insert(SensorId{SensorType::IMU, topics.imu_topic});
   }
   // Odometry is optional.
   if (options.use_odometry) {
-    expected_topics.insert(topics.odometry_topic);
+    expected_topics.insert(
+        SensorId{SensorType::ODOMETRY, topics.odometry_topic});
   }
   // NavSatFix is optional.
   if (options.use_nav_sat) {
-    expected_topics.insert(topics.nav_sat_fix_topic);
+    expected_topics.insert(
+        SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic});
   }
 
   return expected_topics;
@@ -295,16 +300,17 @@ std::unordered_set<std::string> Node::ComputeExpectedTopics(
 
 int Node::AddTrajectory(const TrajectoryOptions& options,
                         const cartographer_ros_msgs::SensorTopics& topics) {
-  const std::unordered_set<std::string> expected_sensor_ids =
+  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-      ComputeExpectedTopics(options, topics);
+      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
   const int trajectory_id =
       map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
   AddExtrapolator(trajectory_id, options);
   AddSensorSamplers(trajectory_id, options);
   LaunchSubscribers(options, topics, trajectory_id);
   is_active_trajectory_[trajectory_id] = true;
-  subscribed_topics_.insert(expected_sensor_ids.begin(),
+  for (const auto& sensor_id : expected_sensor_ids) {
-                            expected_sensor_ids.end());
+    subscribed_topics_.insert(sensor_id.id);
+  }
   return trajectory_id;
 }
 
@@ -384,7 +390,8 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
 bool Node::ValidateTopicNames(
     const ::cartographer_ros_msgs::SensorTopics& topics,
     const TrajectoryOptions& options) {
-  for (const std::string& topic : ComputeExpectedTopics(options, topics)) {
+  for (const auto& sensor_id : ComputeExpectedSensorIds(options, topics)) {
+    const std::string& topic = sensor_id.id;
     if (subscribed_topics_.count(topic) > 0) {
       LOG(ERROR) << "Topic name [" << topic << "] is already used.";
       return false;
@@ -441,14 +448,30 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
   AddTrajectory(options, DefaultSensorTopics());
 }
 
-std::unordered_set<std::string> Node::ComputeDefaultTopics(
+std::vector<
-    const TrajectoryOptions& options) {
+    std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
-  carto::common::MutexLocker lock(&mutex_);
+Node::ComputeDefaultSensorIdsForMultipleBags(
-  return ComputeExpectedTopics(options, DefaultSensorTopics());
+    const std::vector<TrajectoryOptions>& bags_options) const {
+  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
+  std::vector<std::set<SensorId>> bags_sensor_ids;
+  for (size_t i = 0; i < bags_options.size(); ++i) {
+    std::string prefix;
+    if (bags_options.size() > 1) {
+      prefix = "bag_" + std::to_string(i + 1) + "_";
+    }
+    std::set<SensorId> unique_sensor_ids;
+    for (const auto& sensor_id :
+         ComputeExpectedSensorIds(bags_options.at(i), DefaultSensorTopics())) {
+      unique_sensor_ids.insert(SensorId{sensor_id.type, prefix + sensor_id.id});
+    }
+    bags_sensor_ids.push_back(unique_sensor_ids);
+  }
+  return bags_sensor_ids;
 }
 
 int Node::AddOfflineTrajectory(
-    const std::unordered_set<std::string>& expected_sensor_ids,
+    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+        expected_sensor_ids,
     const TrajectoryOptions& options) {
   carto::common::MutexLocker lock(&mutex_);
   const int trajectory_id =

cartographer_ros/cartographer_ros/node.h

@@ -19,6 +19,7 @@
 
 #include <map>
 #include <memory>
+#include <set>
 #include <unordered_map>
 #include <unordered_set>
 #include <vector>
@@ -73,13 +74,19 @@ class Node {
   // Starts the first trajectory with the default topics.
   void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
 
-  // Compute the default topics for the given 'options'.
+  // Returns unique SensorIds for multiple input bag files based on
-  std::unordered_set<std::string> ComputeDefaultTopics(
+  // their TrajectoryOptions.
-      const TrajectoryOptions& options);
+  // 'SensorId::id' is the expected ROS topic name.
+  std::vector<
+      std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
+  ComputeDefaultSensorIdsForMultipleBags(
+      const std::vector<TrajectoryOptions>& bags_options) const;
 
   // Adds a trajectory for offline processing, i.e. not listening to topics.
   int AddOfflineTrajectory(
-      const std::unordered_set<std::string>& expected_sensor_ids,
+      const std::set<
+          cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
+          expected_sensor_ids,
       const TrajectoryOptions& options);
 
   // The following functions handle adding sensor data to a trajectory.
@@ -127,10 +134,12 @@ class Node {
       cartographer_ros_msgs::FinishTrajectory::Response& response);
   bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
                         cartographer_ros_msgs::WriteState::Response& response);
-  // Returns the set of topic names we want to subscribe to.
+  // Returns the set of SensorIds expected for a trajectory.
-  std::unordered_set<std::string> ComputeExpectedTopics(
+  // 'SensorId::id' is the expected ROS topic name.
+  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
+  ComputeExpectedSensorIds(
       const TrajectoryOptions& options,
-      const cartographer_ros_msgs::SensorTopics& topics);
+      const cartographer_ros_msgs::SensorTopics& topics) const;
   int AddTrajectory(const TrajectoryOptions& options,
                     const cartographer_ros_msgs::SensorTopics& topics);
   void LaunchSubscribers(const TrajectoryOptions& options,

cartographer_ros/cartographer_ros/offline_node.cc

@@ -56,15 +56,6 @@ DEFINE_string(pbstream_filename, "",
 DEFINE_bool(keep_running, false,
             "Keep running the offline node after all messages from the bag "
             "have been processed.");
-DEFINE_string(sensor_topics, "",
-              "Optional comma-separated list of colon-separated lists of "
-              "sensor topics for each trajectory. A single trajectory's topics "
-              "are delimited with colons, while different trajectories are "
-              "delimited with commas. If a blank list is given for a "
-              "trajectory, the default topics are used. If a single "
-              "colon-separated list is given, it will be used for all "
-              "trajectories. If omitted, default topics will be used "
-              "for all trajectories.");
 
 namespace cartographer_ros {
 
@@ -160,28 +151,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
       },
       false /* oneshot */, false /* autostart */);
 
-  // Colon-delimited string lists of sensor topics for each bag.
+  auto bag_expected_sensor_ids =
-  std::vector<std::string> bag_sensor_topics_strings;
+      node.ComputeDefaultSensorIdsForMultipleBags(bag_trajectory_options);
-
+  std::map<std::string,
-  if (FLAGS_sensor_topics.empty()) {
+           cartographer::mapping::TrajectoryBuilderInterface::SensorId>
-    // Use default topic names for all bags, denoted by an empty sensor
+      bag_topic_to_sensor_id;
-    // topic list string for each bag.
-    bag_sensor_topics_strings =
-        std::vector<std::string>(bag_filenames.size(), std::string());
-  } else {
-    bag_sensor_topics_strings =
-        cartographer_ros::SplitString(FLAGS_sensor_topics, ',');
-    if (bag_sensor_topics_strings.size() == 1) {
-      // Use the single specified topic list string for all bags.
-      bag_sensor_topics_strings.insert(bag_sensor_topics_strings.end(),
-                                       bag_filenames.size() - 1,
-                                       bag_sensor_topics_strings.at(0));
-    }
-  }
-  CHECK_EQ(bag_sensor_topics_strings.size(), bag_filenames.size());
-
-  std::vector<std::unordered_set<std::string>> bag_sensor_topics;
-  std::unordered_map<int, int> bag_index_to_trajectory_id;
   PlayableBagMultiplexer playable_bag_multiplexer;
   for (size_t current_bag_index = 0; current_bag_index < bag_filenames.size();
        ++current_bag_index) {
@@ -189,25 +163,21 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     if (!::ros::ok()) {
       return;
     }
-    std::vector<std::string> unresolved_current_bag_sensor_topics;
-    if (bag_sensor_topics_strings.at(current_bag_index).empty()) {
-      // Empty topic list string provided for this trajectory,
-      // use default topics.
-      const auto default_topics = node.ComputeDefaultTopics(
-          bag_trajectory_options.at(current_bag_index));
-      unresolved_current_bag_sensor_topics = std::vector<std::string>(
-          default_topics.begin(), default_topics.end());
-    } else {
-      unresolved_current_bag_sensor_topics = cartographer_ros::SplitString(
-          bag_sensor_topics_strings.at(current_bag_index), ':');
-    }
     std::unordered_set<std::string> current_bag_sensor_topics;
-    for (const auto& topic : unresolved_current_bag_sensor_topics) {
+    for (const auto& expected_sensor_id :
-      CHECK(current_bag_sensor_topics
+         bag_expected_sensor_ids.at(current_bag_index)) {
-                .insert(node.node_handle()->resolveName(topic))
+      std::string resolved_topic =
-                .second);
+          node.node_handle()->resolveName(expected_sensor_id.id);
+      if (bag_topic_to_sensor_id.count(resolved_topic) != 0) {
+        LOG(ERROR) << "Sensor " << expected_sensor_id.id
+                   << " resolves to topic " << resolved_topic
+                   << " which is already used by "
+                   << " sensor "
+                   << bag_topic_to_sensor_id.at(resolved_topic).id;
+      }
+      bag_topic_to_sensor_id[resolved_topic] = expected_sensor_id;
+      current_bag_sensor_topics.insert(resolved_topic);
     }
-    bag_sensor_topics.push_back(current_bag_sensor_topics);
 
     playable_bag_multiplexer.AddPlayableBag(PlayableBag(
         bag_filename, current_bag_index, ros::TIME_MIN, ros::TIME_MAX, kDelay,
@@ -243,8 +213,9 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
           }
         }));
   }
-  CHECK_EQ(bag_sensor_topics.size(), bag_filenames.size());
 
+  // TODO(gaschler): Warn if resolved topics are not in bags.
+  std::unordered_map<int, int> bag_index_to_trajectory_id;
   while (playable_bag_multiplexer.IsMessageAvailable()) {
     const auto next_msg_tuple = playable_bag_multiplexer.GetNextMessage();
     const rosbag::MessageInstance& msg = std::get<0>(next_msg_tuple);
@@ -256,7 +227,7 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     // to avoid blocking the sensor queue.
     if (bag_index_to_trajectory_id.count(bag_index) == 0) {
       trajectory_id =
-          node.AddOfflineTrajectory(bag_sensor_topics.at(bag_index),
+          node.AddOfflineTrajectory(bag_expected_sensor_ids.at(bag_index),
                                     bag_trajectory_options.at(bag_index));
       CHECK(bag_index_to_trajectory_id
                 .emplace(std::piecewise_construct,
@@ -272,32 +243,35 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
     if (!::ros::ok()) {
       return;
     }
-    const std::string topic =
+    const std::string bag_topic =
         node.node_handle()->resolveName(msg.getTopic(), false /* resolve */);
-    if (bag_sensor_topics.at(bag_index).count(topic) > 0) {
+    auto it = bag_topic_to_sensor_id.find(bag_topic);
+    if (it != bag_topic_to_sensor_id.end()) {
+      const std::string& sensor_id = it->second.id;
       if (msg.isType<sensor_msgs::LaserScan>()) {
-        node.HandleLaserScanMessage(trajectory_id, topic,
+        node.HandleLaserScanMessage(trajectory_id, sensor_id,
                                     msg.instantiate<sensor_msgs::LaserScan>());
       }
       if (msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
         node.HandleMultiEchoLaserScanMessage(
-            trajectory_id, topic,
+            trajectory_id, sensor_id,
             msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
       }
       if (msg.isType<sensor_msgs::PointCloud2>()) {
         node.HandlePointCloud2Message(
-            trajectory_id, topic, msg.instantiate<sensor_msgs::PointCloud2>());
+            trajectory_id, sensor_id,
+            msg.instantiate<sensor_msgs::PointCloud2>());
       }
       if (msg.isType<sensor_msgs::Imu>()) {
-        node.HandleImuMessage(trajectory_id, topic,
+        node.HandleImuMessage(trajectory_id, sensor_id,
                               msg.instantiate<sensor_msgs::Imu>());
       }
       if (msg.isType<nav_msgs::Odometry>()) {
-        node.HandleOdometryMessage(trajectory_id, topic,
+        node.HandleOdometryMessage(trajectory_id, sensor_id,
                                    msg.instantiate<nav_msgs::Odometry>());
       }
       if (msg.isType<sensor_msgs::NavSatFix>()) {
-        node.HandleNavSatFixMessage(trajectory_id, topic,
+        node.HandleNavSatFixMessage(trajectory_id, sensor_id,
                                     msg.instantiate<sensor_msgs::NavSatFix>());
       }
     }