Crash fix in offline node. (#464)

This adds trajectories at the Node object. It makes sure all
necessary extrapolators exist. Before the offline node would crash
when extrapolating.

Also deduplicates the logic to compute the topics for a trajectory.

cartographer_ros/cartographer_ros/node.cc

@@ -44,6 +44,20 @@
 
 namespace cartographer_ros {
 
+namespace {
+
+cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
+  cartographer_ros_msgs::SensorTopics topics;
+  topics.laser_scan_topic = kLaserScanTopic;
+  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
+  topics.point_cloud2_topic = kPointCloud2Topic;
+  topics.imu_topic = kImuTopic;
+  topics.odometry_topic = kOdometryTopic;
+  return topics;
+}
+
+}  // namespace
+
 namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
@@ -207,7 +221,7 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
     const TrajectoryOptions& options,
     const cartographer_ros_msgs::SensorTopics& topics) {
   std::unordered_set<string> expected_topics;
-
+  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
   for (const string& topic : ComputeRepeatedTopicNames(
            topics.laser_scan_topic, options.num_laser_scans)) {
     expected_topics.insert(topic);
@@ -229,6 +243,7 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
            .use_imu_data())) {
     expected_topics.insert(topics.imu_topic);
   }
+  // Odometry is optional.
   if (options.use_odometry) {
     expected_topics.insert(topics.odometry_topic);
   }
@@ -243,6 +258,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
       map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
   AddExtrapolator(trajectory_id, options);
   LaunchSubscribers(options, topics, trajectory_id);
+  is_active_trajectory_[trajectory_id] = true;
   subscribed_topics_.insert(expected_sensor_ids.begin(),
                             expected_sensor_ids.end());
   return trajectory_id;
@@ -328,8 +344,6 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                       ->HandleOdometryMessage(topic, msg);
                 })));
   }
-
-  is_active_trajectory_[trajectory_id] = true;
 }
 
 bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
@@ -375,25 +389,29 @@ bool Node::HandleStartTrajectory(
     LOG(ERROR) << "Invalid topics.";
     return false;
   }
-
-  const int trajectory_id = AddTrajectory(options, request.topics);
-  response.trajectory_id = trajectory_id;
-
-  is_active_trajectory_[trajectory_id] = true;
+  response.trajectory_id = AddTrajectory(options, request.topics);
   return true;
 }
 
 void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
   carto::common::MutexLocker lock(&mutex_);
-  cartographer_ros_msgs::SensorTopics topics;
-  topics.laser_scan_topic = kLaserScanTopic;
-  topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic;
-  topics.point_cloud2_topic = kPointCloud2Topic;
-  topics.imu_topic = kImuTopic;
-  topics.odometry_topic = kOdometryTopic;
+  AddTrajectory(options, DefaultSensorTopics());
+}
 
-  const int trajectory_id = AddTrajectory(options, topics);
-  is_active_trajectory_[trajectory_id] = true;
+std::unordered_set<string> Node::ComputeDefaultTopics(
+    const TrajectoryOptions& options) {
+  carto::common::MutexLocker lock(&mutex_);
+  return ComputeExpectedTopics(options, DefaultSensorTopics());
+}
+
+int Node::AddOfflineTrajectory(
+    const std::unordered_set<string>& expected_sensor_ids,
+    const TrajectoryOptions& options) {
+  carto::common::MutexLocker lock(&mutex_);
+  const int trajectory_id =
+      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+  AddExtrapolator(trajectory_id, options);
+  return trajectory_id;
 }
 
 bool Node::HandleFinishTrajectory(

cartographer_ros/cartographer_ros/node.h

@@ -57,6 +57,15 @@ class Node {
   // Starts the first trajectory with the default topics.
   void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
 
+  // Compute the default topics for the given 'options'.
+  std::unordered_set<string> ComputeDefaultTopics(
+      const TrajectoryOptions& options);
+
+  // Adds a trajectory for offline processing, i.e. not listening to topics.
+  int AddOfflineTrajectory(
+      const std::unordered_set<string>& expected_sensor_ids,
+      const TrajectoryOptions& options);
+
   // Loads a persisted state to use as a map.
   void LoadMap(const std::string& map_filename);
 

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -110,39 +110,9 @@ void Run(const std::vector<string>& bag_filenames) {
   }
 
   std::unordered_set<string> expected_sensor_ids;
-  const auto check_insert = [&expected_sensor_ids, &node](const string& topic) {
+  for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) {
     CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic))
               .second);
-  };
-
-  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
-  for (const string& topic : ComputeRepeatedTopicNames(
-           kLaserScanTopic, trajectory_options.num_laser_scans)) {
-    check_insert(topic);
-  }
-  for (const string& topic : ComputeRepeatedTopicNames(
-           kMultiEchoLaserScanTopic,
-           trajectory_options.num_multi_echo_laser_scans)) {
-    check_insert(topic);
-  }
-  for (const string& topic : ComputeRepeatedTopicNames(
-           kPointCloud2Topic, trajectory_options.num_point_clouds)) {
-    check_insert(topic);
-  }
-
-  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  // required.
-  if (node_options.map_builder_options.use_trajectory_builder_3d() ||
-      (node_options.map_builder_options.use_trajectory_builder_2d() &&
-       trajectory_options.trajectory_builder_options
-           .trajectory_builder_2d_options()
-           .use_imu_data())) {
-    check_insert(kImuTopic);
-  }
-
-  // Odometry is optional.
-  if (trajectory_options.use_odometry) {
-    check_insert(kOdometryTopic);
   }
 
   ::ros::Publisher tf_publisher =
@@ -164,8 +134,8 @@ void Run(const std::vector<string>& bag_filenames) {
       break;
     }
 
-    const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
-        expected_sensor_ids, trajectory_options);
+    const int trajectory_id =
+        node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options);
 
     rosbag::Bag bag;
     bag.open(bag_filename, rosbag::bagmode::Read);