Enable multi-trajectory on ROS node (#346)

cartographer_ros/cartographer_ros/configuration_files_test.cc

@@ -37,6 +37,7 @@ TEST_P(ConfigurationFilesTest, ValidateNodeOptions) {
     ::cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
         code, std::move(file_resolver));
     ::cartographer_ros::CreateNodeOptions(&lua_parameter_dictionary);
+    ::cartographer_ros::CreateTrajectoryOptions(&lua_parameter_dictionary);
   });
 }
 

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -23,24 +23,28 @@
 
 namespace cartographer_ros {
 
-MapBuilderBridge::MapBuilderBridge(const NodeOptions& options,
+MapBuilderBridge::MapBuilderBridge(const NodeOptions& node_options,
                                    tf2_ros::Buffer* const tf_buffer)
-    : options_(options),
+    : node_options_(node_options),
-      map_builder_(options.map_builder_options),
+      map_builder_(node_options.map_builder_options),
       tf_buffer_(tf_buffer) {}
 
 int MapBuilderBridge::AddTrajectory(
     const std::unordered_set<string>& expected_sensor_ids,
-    const string& tracking_frame) {
+    const TrajectoryOptions& trajectory_options) {
   const int trajectory_id = map_builder_.AddTrajectoryBuilder(
-      expected_sensor_ids, options_.trajectory_builder_options);
+      expected_sensor_ids, trajectory_options.trajectory_builder_options);
   LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
 
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
   sensor_bridges_[trajectory_id] =
       cartographer::common::make_unique<SensorBridge>(
-          tracking_frame, options_.lookup_transform_timeout_sec, tf_buffer_,
+          trajectory_options.tracking_frame,
+          node_options_.lookup_transform_timeout_sec, tf_buffer_,
           map_builder_.GetTrajectoryBuilder(trajectory_id));
+  auto emplace_result =
+      trajectory_options_.emplace(trajectory_id, trajectory_options);
+  CHECK(emplace_result.second == true);
   return trajectory_id;
 }
 
@@ -64,18 +68,22 @@ void MapBuilderBridge::WriteAssets(const string& stem) {
     LOG(WARNING) << "No data was collected and no assets will be written.";
   } else {
     LOG(INFO) << "Writing assets with stem '" << stem << "'...";
-    if (options_.map_builder_options.use_trajectory_builder_2d()) {
+    // TODO(yutakaoka): Add multi-trajectory support.
+    CHECK_EQ(trajectory_options_.count(0), 1);
+    if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
       Write2DAssets(
-          trajectory_nodes, options_.map_frame,
+          trajectory_nodes, node_options_.map_frame,
-          options_.trajectory_builder_options.trajectory_builder_2d_options()
+          trajectory_options_[0]
+              .trajectory_builder_options.trajectory_builder_2d_options()
               .submaps_options(),
           stem);
     }
 
-    if (options_.map_builder_options.use_trajectory_builder_3d()) {
+    if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
       Write3DAssets(
           trajectory_nodes,
-          options_.trajectory_builder_options.trajectory_builder_3d_options()
+          trajectory_options_[0]
+              .trajectory_builder_options.trajectory_builder_3d_options()
               .submaps_options()
               .high_resolution(),
           stem);
@@ -108,7 +116,7 @@ bool MapBuilderBridge::HandleSubmapQuery(
 cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
   cartographer_ros_msgs::SubmapList submap_list;
   submap_list.header.stamp = ::ros::Time::now();
-  submap_list.header.frame_id = options_.map_frame;
+  submap_list.header.frame_id = node_options_.map_frame;
   for (int trajectory_id = 0;
        trajectory_id < map_builder_.num_trajectory_builders();
        ++trajectory_id) {
@@ -133,7 +141,7 @@ cartographer_ros_msgs::SubmapList MapBuilderBridge::GetSubmapList() {
 
 std::unique_ptr<nav_msgs::OccupancyGrid>
 MapBuilderBridge::BuildOccupancyGrid() {
-  CHECK(options_.map_builder_options.use_trajectory_builder_2d())
+  CHECK(node_options_.map_builder_options.use_trajectory_builder_2d())
       << "Publishing OccupancyGrids for 3D data is not yet supported";
   std::vector<::cartographer::mapping::TrajectoryNode> trajectory_nodes;
   for (const auto& single_trajectory :
@@ -145,9 +153,11 @@ MapBuilderBridge::BuildOccupancyGrid() {
   if (!trajectory_nodes.empty()) {
     occupancy_grid =
         cartographer::common::make_unique<nav_msgs::OccupancyGrid>();
+    CHECK_EQ(trajectory_options_.count(0), 1);
     BuildOccupancyGrid2D(
-        trajectory_nodes, options_.map_frame,
+        trajectory_nodes, node_options_.map_frame,
-        options_.trajectory_builder_options.trajectory_builder_2d_options()
+        trajectory_options_[0]
+            .trajectory_builder_options.trajectory_builder_2d_options()
             .submaps_options(),
         occupancy_grid.get());
   }
@@ -169,12 +179,15 @@ MapBuilderBridge::GetTrajectoryStates() {
       continue;
     }
 
+    CHECK_EQ(trajectory_options_.count(trajectory_id), 1);
     trajectory_states[trajectory_id] = {
         pose_estimate,
         map_builder_.sparse_pose_graph()->GetLocalToGlobalTransform(
             trajectory_id),
-        sensor_bridge.tf_bridge().LookupToTracking(pose_estimate.time,
+        sensor_bridge.tf_bridge().LookupToTracking(
-                                                   options_.published_frame)};
+            pose_estimate.time,
+            trajectory_options_[trajectory_id].published_frame),
+        trajectory_options_[trajectory_id]};
   }
   return trajectory_states;
 }

cartographer_ros/cartographer_ros/map_builder_bridge.h

@@ -26,6 +26,7 @@
 #include "cartographer_ros/node_options.h"
 #include "cartographer_ros/sensor_bridge.h"
 #include "cartographer_ros/tf_bridge.h"
+#include "cartographer_ros/trajectory_options.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
@@ -39,15 +40,16 @@ class MapBuilderBridge {
     cartographer::mapping::TrajectoryBuilder::PoseEstimate pose_estimate;
     cartographer::transform::Rigid3d local_to_map;
     std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
+    TrajectoryOptions trajectory_options;
   };
 
-  MapBuilderBridge(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
+  MapBuilderBridge(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
 
   MapBuilderBridge(const MapBuilderBridge&) = delete;
   MapBuilderBridge& operator=(const MapBuilderBridge&) = delete;
 
   int AddTrajectory(const std::unordered_set<string>& expected_sensor_ids,
-                    const string& tracking_frame);
+                    const TrajectoryOptions& trajectory_options);
   void FinishTrajectory(int trajectory_id);
   void WriteAssets(const string& stem);
 
@@ -62,10 +64,12 @@ class MapBuilderBridge {
   SensorBridge* sensor_bridge(int trajectory_id);
 
  private:
-  const NodeOptions options_;
+  const NodeOptions node_options_;
-
   cartographer::mapping::MapBuilder map_builder_;
   tf2_ros::Buffer* const tf_buffer_;
+
+  // These are keyed with 'trajectory_id'.
+  std::unordered_map<int, TrajectoryOptions> trajectory_options_;
   std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;
 };
 

cartographer_ros/cartographer_ros/node.cc

@@ -21,6 +21,8 @@
 #include <vector>
 
 #include "Eigen/Core"
+#include "cartographer/common/configuration_file_resolver.h"
+#include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
@@ -45,30 +47,59 @@ namespace carto = ::cartographer;
 
 using carto::transform::Rigid3d;
 
+namespace {
+
+constexpr int kInfiniteSubscriberQueueSize = 0;
 constexpr int kLatestOnlyPublisherQueueSize = 1;
 
-Node::Node(const NodeOptions& options, tf2_ros::Buffer* const tf_buffer)
+TrajectoryOptions ToTrajectoryOptions(
-    : options_(options), map_builder_bridge_(options_, tf_buffer) {}
+    cartographer_ros_msgs::TrajectoryOptions ros_options) {
-
+  TrajectoryOptions options;
-Node::~Node() {
+  options.tracking_frame = ros_options.tracking_frame;
-  {
+  options.published_frame = ros_options.published_frame;
-    carto::common::MutexLocker lock(&mutex_);
+  options.odom_frame = ros_options.odom_frame;
-    terminating_ = true;
+  options.provide_odom_frame = ros_options.provide_odom_frame;
-  }
+  options.use_odometry = ros_options.use_odometry;
-  if (occupancy_grid_thread_.joinable()) {
+  options.use_laser_scan = ros_options.use_laser_scan;
-    occupancy_grid_thread_.join();
+  options.use_multi_echo_laser_scan = ros_options.use_multi_echo_laser_scan;
+  options.num_point_clouds = ros_options.num_point_clouds;
+  if (!options.trajectory_builder_options.ParseFromString(
+          ros_options.trajectory_builder_options_proto)) {
+    ROS_ERROR("Failed to parse protobuf");
   }
+  return options;
 }
 
-void Node::Initialize() {
+void ShutdownSubscriber(std::unordered_map<int, ::ros::Subscriber>& subscribers,
+                        int trajectory_id) {
+  if (subscribers.count(trajectory_id) == 0) {
+    return;
+  }
+  subscribers[trajectory_id].shutdown();
+  ROS_INFO_STREAM("Shutdown the subscriber of ["
+                  << subscribers[trajectory_id].getTopic() << "]");
+  CHECK_EQ(subscribers.erase(trajectory_id), 1);
+}
+
+}  // namespace
+
+Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer)
+    : node_options_(node_options),
+      map_builder_bridge_(node_options_, tf_buffer) {
   carto::common::MutexLocker lock(&mutex_);
   submap_list_publisher_ =
       node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
           kSubmapListTopic, kLatestOnlyPublisherQueueSize);
-  submap_query_server_ = node_handle_.advertiseService(
+  service_servers_.push_back(node_handle_.advertiseService(
-      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this);
+      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
+  service_servers_.push_back(node_handle_.advertiseService(
+      kWriteAssetsServiceName, &Node::HandleWriteAssets, this));
 
-  if (options_.map_builder_options.use_trajectory_builder_2d()) {
+  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
     occupancy_grid_publisher_ =
         node_handle_.advertise<::nav_msgs::OccupancyGrid>(
             kOccupancyGridTopic, kLatestOnlyPublisherQueueSize,
@@ -82,13 +113,23 @@ void Node::Initialize() {
           kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
 
   wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(options_.submap_publish_period_sec),
+      ::ros::WallDuration(node_options_.submap_publish_period_sec),
       &Node::PublishSubmapList, this));
   wall_timers_.push_back(node_handle_.createWallTimer(
-      ::ros::WallDuration(options_.pose_publish_period_sec),
+      ::ros::WallDuration(node_options_.pose_publish_period_sec),
       &Node::PublishTrajectoryStates, this));
 }
 
+Node::~Node() {
+  {
+    carto::common::MutexLocker lock(&mutex_);
+    terminating_ = true;
+  }
+  if (occupancy_grid_thread_.joinable()) {
+    occupancy_grid_thread_.join();
+  }
+}
+
 ::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
 
 MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
@@ -123,7 +164,7 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
         last_scan_matched_point_cloud_time_) {
       scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
           carto::common::ToUniversal(trajectory_state.pose_estimate.time),
-          options_.tracking_frame,
+          trajectory_state.trajectory_options.tracking_frame,
           carto::sensor::TransformPointCloud(
               trajectory_state.pose_estimate.point_cloud,
               tracking_to_local.inverse().cast<float>())));
@@ -135,27 +176,29 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
     }
 
     if (trajectory_state.published_to_tracking != nullptr) {
-      if (options_.provide_odom_frame) {
+      if (trajectory_state.trajectory_options.provide_odom_frame) {
         std::vector<geometry_msgs::TransformStamped> stamped_transforms;
 
-        stamped_transform.header.frame_id = options_.map_frame;
+        stamped_transform.header.frame_id = node_options_.map_frame;
-        // TODO(damonkohler): 'odom_frame' and 'published_frame' must be
+        stamped_transform.child_frame_id =
-        // per-trajectory to fully support the multi-robot use case.
+            trajectory_state.trajectory_options.odom_frame;
-        stamped_transform.child_frame_id = options_.odom_frame;
         stamped_transform.transform =
             ToGeometryMsgTransform(trajectory_state.local_to_map);
         stamped_transforms.push_back(stamped_transform);
 
-        stamped_transform.header.frame_id = options_.odom_frame;
+        stamped_transform.header.frame_id =
-        stamped_transform.child_frame_id = options_.published_frame;
+            trajectory_state.trajectory_options.odom_frame;
+        stamped_transform.child_frame_id =
+            trajectory_state.trajectory_options.published_frame;
         stamped_transform.transform = ToGeometryMsgTransform(
             tracking_to_local * (*trajectory_state.published_to_tracking));
         stamped_transforms.push_back(stamped_transform);
 
         tf_broadcaster_.sendTransform(stamped_transforms);
       } else {
-        stamped_transform.header.frame_id = options_.map_frame;
+        stamped_transform.header.frame_id = node_options_.map_frame;
-        stamped_transform.child_frame_id = options_.published_frame;
+        stamped_transform.child_frame_id =
+            trajectory_state.trajectory_options.published_frame;
         stamped_transform.transform = ToGeometryMsgTransform(
             tracking_to_map * (*trajectory_state.published_to_tracking));
         tf_broadcaster_.sendTransform(stamped_transform);
@@ -183,4 +226,192 @@ void Node::SpinOccupancyGridThreadForever() {
   }
 }
 
+int Node::AddTrajectory(const TrajectoryOptions& options,
+                        const cartographer_ros_msgs::SensorTopics& topics) {
+  std::unordered_set<string> expected_sensor_ids;
+
+  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
+    // Using point clouds is only supported in 3D.
+    CHECK_EQ(options.num_point_clouds, 0);
+  }
+  if (options.use_laser_scan) {
+    expected_sensor_ids.insert(topics.laser_scan_topic);
+  }
+  if (options.use_multi_echo_laser_scan) {
+    expected_sensor_ids.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);
+    }
+  }
+  if (options.trajectory_builder_options.trajectory_builder_2d_options()
+          .use_imu_data()) {
+    expected_sensor_ids.insert(topics.imu_topic);
+  }
+  if (options.use_odometry) {
+    expected_sensor_ids.insert(topics.odometry_topic);
+  }
+  return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
+}
+
+void Node::LaunchSubscribers(const TrajectoryOptions& options,
+                             const cartographer_ros_msgs::SensorTopics& topics,
+                             const int trajectory_id) {
+  if (options.use_laser_scan) {
+    const string topic = topics.laser_scan_topic;
+    laser_scan_subscribers_[trajectory_id] =
+        node_handle_.subscribe<sensor_msgs::LaserScan>(
+            topic, kInfiniteSubscriberQueueSize,
+            boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
+                [this, trajectory_id,
+                 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] =
+        node_handle_.subscribe<sensor_msgs::MultiEchoLaserScan>(
+            topic, kInfiniteSubscriberQueueSize,
+            boost::function<void(
+                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;
+  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);
+      }
+      grouped_point_cloud_subscribers.push_back(node_handle_.subscribe(
+          topic, kInfiniteSubscriberQueueSize,
+          boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
+              [this, trajectory_id,
+               topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
+                map_builder_bridge_.sensor_bridge(trajectory_id)
+                    ->HandlePointCloud2Message(topic, msg);
+              })));
+    }
+    point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers;
+  }
+
+  if (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>(
+        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] =
+        node_handle_.subscribe<nav_msgs::Odometry>(
+            topic, kInfiniteSubscriberQueueSize,
+            boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
+                [this, trajectory_id,
+                 topic](const nav_msgs::Odometry::ConstPtr& msg) {
+                  map_builder_bridge_.sensor_bridge(trajectory_id)
+                      ->HandleOdometryMessage(topic, msg);
+                }));
+  }
+
+  is_active_trajectory_[trajectory_id] = true;
+}
+
+bool Node::HandleStartTrajectory(
+    ::cartographer_ros_msgs::StartTrajectory::Request& request,
+    ::cartographer_ros_msgs::StartTrajectory::Response& response) {
+  carto::common::MutexLocker lock(&mutex_);
+  std::unordered_set<string> expected_sensor_ids;
+  TrajectoryOptions options = ToTrajectoryOptions(request.options);
+  const int trajectory_id = AddTrajectory(options, request.topics);
+  LaunchSubscribers(options, request.topics, trajectory_id);
+
+  is_active_trajectory_[trajectory_id] = true;
+  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;
+
+  const int trajectory_id = AddTrajectory(options, topics);
+  LaunchSubscribers(options, topics, trajectory_id);
+}
+
+bool Node::HandleFinishTrajectory(
+    ::cartographer_ros_msgs::FinishTrajectory::Request& request,
+    ::cartographer_ros_msgs::FinishTrajectory::Response& response) {
+  carto::common::MutexLocker lock(&mutex_);
+  const int trajectory_id = request.trajectory_id;
+  if (is_active_trajectory_.count(trajectory_id) == 0) {
+    ROS_INFO_STREAM("Trajectory_id " << trajectory_id
+                                     << " is not created yet.");
+    return false;
+  }
+  if (!is_active_trajectory_[trajectory_id]) {
+    ROS_INFO_STREAM("Trajectory_id " << trajectory_id
+                                     << " has already been finished.");
+    return false;
+  }
+
+  ShutdownSubscriber(laser_scan_subscribers_, trajectory_id);
+  ShutdownSubscriber(multi_echo_laser_scan_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]) {
+      ROS_INFO_STREAM("Shutdown the subscriber of [" << entry.getTopic()
+                                                     << "]");
+      entry.shutdown();
+    }
+    CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1);
+  }
+  map_builder_bridge_.FinishTrajectory(trajectory_id);
+  is_active_trajectory_[trajectory_id] = false;
+  return true;
+}
+
+bool Node::HandleWriteAssets(
+    ::cartographer_ros_msgs::WriteAssets::Request& request,
+    ::cartographer_ros_msgs::WriteAssets::Response& response) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.WriteAssets(request.stem);
+  return true;
+}
+
+void Node::FinishAllTrajectories() {
+  carto::common::MutexLocker lock(&mutex_);
+  for (const auto& entry : is_active_trajectory_) {
+    const int trajectory_id = entry.first;
+    if (entry.second) {
+      map_builder_bridge_.FinishTrajectory(trajectory_id);
+    }
+  }
+}
 }  // namespace cartographer_ros

cartographer_ros/cartographer_ros/node.h

@@ -23,11 +23,16 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer_ros/map_builder_bridge.h"
 #include "cartographer_ros/node_options.h"
+#include "cartographer_ros/trajectory_options.h"
 #include "cartographer_ros_msgs/FinishTrajectory.h"
+#include "cartographer_ros_msgs/SensorTopics.h"
+#include "cartographer_ros_msgs/StartTrajectory.h"
 #include "cartographer_ros_msgs/SubmapEntry.h"
 #include "cartographer_ros_msgs/SubmapList.h"
 #include "cartographer_ros_msgs/SubmapQuery.h"
+#include "cartographer_ros_msgs/TrajectoryOptions.h"
 #include "cartographer_ros_msgs/TrajectorySubmapList.h"
+#include "cartographer_ros_msgs/WriteAssets.h"
 #include "ros/ros.h"
 #include "tf2_ros/transform_broadcaster.h"
 
@@ -44,17 +49,23 @@ constexpr char kOccupancyGridTopic[] = "map";
 constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
 constexpr char kSubmapListTopic[] = "submap_list";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
+constexpr char kStartTrajectoryServiceName[] = "start_trajectory";
+constexpr char kWriteAssetsServiceName[] = "write_assets";
 
 // Wires up ROS topics to SLAM.
 class Node {
  public:
-  Node(const NodeOptions& options, tf2_ros::Buffer* tf_buffer);
+  Node(const NodeOptions& node_options, tf2_ros::Buffer* tf_buffer);
   ~Node();
 
   Node(const Node&) = delete;
   Node& operator=(const Node&) = delete;
 
-  void Initialize();
+  // Finishes all yet active trajectories.
+  void FinishAllTrajectories();
+
+  // Starts the first trajectory with the default topics.
+  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
 
   ::ros::NodeHandle* node_handle();
   MapBuilderBridge* map_builder_bridge();
@@ -63,27 +74,47 @@ class Node {
   bool HandleSubmapQuery(
       cartographer_ros_msgs::SubmapQuery::Request& request,
       cartographer_ros_msgs::SubmapQuery::Response& response);
-
+  bool HandleStartTrajectory(
+      cartographer_ros_msgs::StartTrajectory::Request& request,
+      cartographer_ros_msgs::StartTrajectory::Response& response);
+  bool HandleFinishTrajectory(
+      cartographer_ros_msgs::FinishTrajectory::Request& request,
+      cartographer_ros_msgs::FinishTrajectory::Response& response);
+  bool HandleWriteAssets(
+      cartographer_ros_msgs::WriteAssets::Request& request,
+      cartographer_ros_msgs::WriteAssets::Response& response);
+  int AddTrajectory(const TrajectoryOptions& options,
+                    const cartographer_ros_msgs::SensorTopics& topics);
+  void LaunchSubscribers(const TrajectoryOptions& options,
+                         const cartographer_ros_msgs::SensorTopics& topics,
+                         int trajectory_id);
   void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
   void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
   void SpinOccupancyGridThreadForever();
 
-  const NodeOptions options_;
+  const NodeOptions node_options_;
 
   tf2_ros::TransformBroadcaster tf_broadcaster_;
 
   cartographer::common::Mutex mutex_;
   MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
-  int trajectory_id_ = -1;
-  std::unordered_set<string> expected_sensor_ids_;
 
   ::ros::NodeHandle node_handle_;
   ::ros::Publisher submap_list_publisher_;
-  ::ros::ServiceServer submap_query_server_;
+  // These ros::ServiceServers need to live for the lifetime of the node.
+  std::vector<::ros::ServiceServer> service_servers_;
   ::ros::Publisher scan_matched_point_cloud_publisher_;
   cartographer::common::Time last_scan_matched_point_cloud_time_ =
       cartographer::common::Time::min();
 
+  // These are keyed with 'trajectory_id'.
+  std::unordered_map<int, ::ros::Subscriber> laser_scan_subscribers_;
+  std::unordered_map<int, ::ros::Subscriber> multi_echo_laser_scan_subscribers_;
+  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_;
   bool terminating_ = false GUARDED_BY(mutex_);

cartographer_ros/cartographer_ros/node_main.cc

@@ -22,6 +22,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer_ros/node.h"
+#include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "gflags/gflags.h"
 #include "tf2_ros/transform_listener.h"
@@ -37,9 +38,7 @@ DEFINE_string(configuration_basename, "",
 namespace cartographer_ros {
 namespace {
 
-constexpr int kInfiniteSubscriberQueueSize = 0;
+std::tuple<NodeOptions, TrajectoryOptions> LoadOptions() {
-
-NodeOptions LoadOptions() {
   auto file_resolver = cartographer::common::make_unique<
       cartographer::common::ConfigurationFileResolver>(
       std::vector<string>{FLAGS_configuration_directory});
@@ -48,121 +47,24 @@ NodeOptions LoadOptions() {
   cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
 
-  return CreateNodeOptions(&lua_parameter_dictionary);
+  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
+                         CreateTrajectoryOptions(&lua_parameter_dictionary));
 }
 
 void Run() {
-  const auto options = LoadOptions();
   constexpr double kTfBufferCacheTimeInSeconds = 1e6;
   tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
   tf2_ros::TransformListener tf(tf_buffer);
-  Node node(options, &tf_buffer);
+  NodeOptions node_options;
-  node.Initialize();
+  TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) = LoadOptions();
 
-  int trajectory_id = -1;
+  Node node(node_options, &tf_buffer);
-  std::unordered_set<string> expected_sensor_ids;
+  node.StartTrajectoryWithDefaultTopics(trajectory_options);
-
-  // For 2D SLAM, subscribe to exactly one horizontal laser.
-  ::ros::Subscriber laser_scan_subscriber;
-  if (options.use_laser_scan) {
-    laser_scan_subscriber = node.node_handle()->subscribe(
-        kLaserScanTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const sensor_msgs::LaserScan::ConstPtr&)>(
-            [&](const sensor_msgs::LaserScan::ConstPtr& msg) {
-              node.map_builder_bridge()
-                  ->sensor_bridge(trajectory_id)
-                  ->HandleLaserScanMessage(kLaserScanTopic, msg);
-            }));
-    expected_sensor_ids.insert(kLaserScanTopic);
-  }
-  if (options.use_multi_echo_laser_scan) {
-    laser_scan_subscriber = node.node_handle()->subscribe(
-        kMultiEchoLaserScanTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const sensor_msgs::MultiEchoLaserScan::ConstPtr&)>(
-            [&](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-              node.map_builder_bridge()
-                  ->sensor_bridge(trajectory_id)
-                  ->HandleMultiEchoLaserScanMessage(kMultiEchoLaserScanTopic,
-                                                    msg);
-            }));
-    expected_sensor_ids.insert(kMultiEchoLaserScanTopic);
-  }
-
-  // For 3D SLAM, subscribe to all point clouds topics.
-  std::vector<::ros::Subscriber> point_cloud_subscribers;
-  if (options.num_point_clouds > 0) {
-    for (int i = 0; i < options.num_point_clouds; ++i) {
-      string topic = kPointCloud2Topic;
-      if (options.num_point_clouds > 1) {
-        topic += "_" + std::to_string(i + 1);
-      }
-      point_cloud_subscribers.push_back(node.node_handle()->subscribe(
-          topic, kInfiniteSubscriberQueueSize,
-          boost::function<void(const sensor_msgs::PointCloud2::ConstPtr&)>(
-              [&, topic](const sensor_msgs::PointCloud2::ConstPtr& msg) {
-                node.map_builder_bridge()
-                    ->sensor_bridge(trajectory_id)
-                    ->HandlePointCloud2Message(topic, msg);
-              })));
-      expected_sensor_ids.insert(topic);
-    }
-  }
-
-  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
-  // required.
-  ::ros::Subscriber imu_subscriber;
-  if (options.map_builder_options.use_trajectory_builder_3d() ||
-      (options.map_builder_options.use_trajectory_builder_2d() &&
-       options.trajectory_builder_options.trajectory_builder_2d_options()
-           .use_imu_data())) {
-    imu_subscriber = node.node_handle()->subscribe(
-        kImuTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const sensor_msgs::Imu::ConstPtr& msg)>(
-            [&](const sensor_msgs::Imu::ConstPtr& msg) {
-              node.map_builder_bridge()
-                  ->sensor_bridge(trajectory_id)
-                  ->HandleImuMessage(kImuTopic, msg);
-            }));
-    expected_sensor_ids.insert(kImuTopic);
-  }
-
-  // For both 2D and 3D SLAM, odometry is optional.
-  ::ros::Subscriber odometry_subscriber;
-  if (options.use_odometry) {
-    odometry_subscriber = node.node_handle()->subscribe(
-        kOdometryTopic, kInfiniteSubscriberQueueSize,
-        boost::function<void(const nav_msgs::Odometry::ConstPtr&)>(
-            [&](const nav_msgs::Odometry::ConstPtr& msg) {
-              node.map_builder_bridge()
-                  ->sensor_bridge(trajectory_id)
-                  ->HandleOdometryMessage(kOdometryTopic, msg);
-            }));
-    expected_sensor_ids.insert(kOdometryTopic);
-  }
-
-  trajectory_id = node.map_builder_bridge()->AddTrajectory(
-      expected_sensor_ids, options.tracking_frame);
-
-  ::ros::ServiceServer finish_trajectory_server =
-      node.node_handle()->advertiseService(
-          kFinishTrajectoryServiceName,
-          boost::function<bool(
-              ::cartographer_ros_msgs::FinishTrajectory::Request&,
-              ::cartographer_ros_msgs::FinishTrajectory::Response&)>(
-              [&](::cartographer_ros_msgs::FinishTrajectory::Request& request,
-                  ::cartographer_ros_msgs::FinishTrajectory::Response&) {
-                const int previous_trajectory_id = trajectory_id;
-                trajectory_id = node.map_builder_bridge()->AddTrajectory(
-                    expected_sensor_ids, options.tracking_frame);
-                node.map_builder_bridge()->FinishTrajectory(
-                    previous_trajectory_id);
-                node.map_builder_bridge()->WriteAssets(request.stem);
-                return true;
-              }));
 
   ::ros::spin();
 
-  node.map_builder_bridge()->FinishTrajectory(trajectory_id);
+  node.FinishAllTrajectories();
 }
 
 }  // namespace

cartographer_ros/cartographer_ros/node_options.cc

@@ -27,23 +27,7 @@ NodeOptions CreateNodeOptions(
   options.map_builder_options =
       ::cartographer::mapping::CreateMapBuilderOptions(
           lua_parameter_dictionary->GetDictionary("map_builder").get());
-  options.trajectory_builder_options =
-      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
-          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
   options.map_frame = lua_parameter_dictionary->GetString("map_frame");
-  options.tracking_frame =
-      lua_parameter_dictionary->GetString("tracking_frame");
-  options.published_frame =
-      lua_parameter_dictionary->GetString("published_frame");
-  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
-  options.provide_odom_frame =
-      lua_parameter_dictionary->GetBool("provide_odom_frame");
-  options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
-  options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
-  options.use_multi_echo_laser_scan =
-      lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
-  options.num_point_clouds =
-      lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
   options.lookup_transform_timeout_sec =
       lua_parameter_dictionary->GetDouble("lookup_transform_timeout_sec");
   options.submap_publish_period_sec =
@@ -51,17 +35,6 @@ NodeOptions CreateNodeOptions(
   options.pose_publish_period_sec =
       lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
 
-  CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
-               (options.num_point_clouds > 0),
-           1)
-      << "Configuration error: 'use_laser_scan', "
-         "'use_multi_echo_laser_scan' and 'num_point_clouds' are "
-         "mutually exclusive, but one is required.";
-
-  if (options.map_builder_options.use_trajectory_builder_2d()) {
-    // Using point clouds is only supported in 3D.
-    CHECK_EQ(options.num_point_clouds, 0);
-  }
   return options;
 }
 

cartographer_ros/cartographer_ros/node_options.h

@@ -20,26 +20,15 @@
 #include <string>
 
 #include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/common/port.h"
 #include "cartographer/mapping/map_builder.h"
-#include "cartographer_ros/sensor_bridge.h"
+#include "cartographer_ros/trajectory_options.h"
 
 namespace cartographer_ros {
 
 // Top-level options of Cartographer's ROS integration.
 struct NodeOptions {
   ::cartographer::mapping::proto::MapBuilderOptions map_builder_options;
-  ::cartographer::mapping::proto::TrajectoryBuilderOptions
-      trajectory_builder_options;
   string map_frame;
-  string tracking_frame;
-  string published_frame;
-  string odom_frame;
-  bool provide_odom_frame;
-  bool use_odometry;
-  bool use_laser_scan;
-  bool use_multi_echo_laser_scan;
-  int num_point_clouds;
   double lookup_transform_timeout_sec;
   double submap_publish_period_sec;
   double pose_publish_period_sec;

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -24,6 +24,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/port.h"
 #include "cartographer_ros/node.h"
+#include "cartographer_ros/node_options.h"
 #include "cartographer_ros/ros_log_sink.h"
 #include "cartographer_ros/urdf_reader.h"
 #include "gflags/gflags.h"
@@ -71,7 +72,9 @@ std::vector<string> SplitString(const string& input, const char delimiter) {
   return tokens;
 }
 
-NodeOptions LoadOptions() {
+// TODO(hrapp): This is duplicated in node_main.cc. Pull out into a config
+// unit.
+std::tuple<NodeOptions, TrajectoryOptions> LoadOptions() {
   auto file_resolver = cartographer::common::make_unique<
       cartographer::common::ConfigurationFileResolver>(
       std::vector<string>{FLAGS_configuration_directory});
@@ -80,11 +83,14 @@ NodeOptions LoadOptions() {
   cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
       code, std::move(file_resolver));
 
-  return CreateNodeOptions(&lua_parameter_dictionary);
+  return std::make_tuple(CreateNodeOptions(&lua_parameter_dictionary),
+                         CreateTrajectoryOptions(&lua_parameter_dictionary));
 }
 
 void Run(const std::vector<string>& bag_filenames) {
-  auto options = LoadOptions();
+  NodeOptions node_options;
+  TrajectoryOptions trajectory_options;
+  std::tie(node_options, trajectory_options) = LoadOptions();
 
   tf2_ros::Buffer tf_buffer;
 
@@ -99,9 +105,8 @@ void Run(const std::vector<string>& bag_filenames) {
   // Since we preload the transform buffer, we should never have to wait for a
   // transform. When we finish processing the bag, we will simply drop any
   // remaining sensor data that cannot be transformed due to missing transforms.
-  options.lookup_transform_timeout_sec = 0.;
+  node_options.lookup_transform_timeout_sec = 0.;
-  Node node(options, &tf_buffer);
+  Node node(node_options, &tf_buffer);
-  node.Initialize();
 
   std::unordered_set<string> expected_sensor_ids;
   const auto check_insert = [&expected_sensor_ids, &node](const string& topic) {
@@ -110,18 +115,19 @@ void Run(const std::vector<string>& bag_filenames) {
   };
 
   // For 2D SLAM, subscribe to exactly one horizontal laser.
-  if (options.use_laser_scan) {
+  if (trajectory_options.use_laser_scan) {
     check_insert(kLaserScanTopic);
   }
-  if (options.use_multi_echo_laser_scan) {
+  if (trajectory_options.use_multi_echo_laser_scan) {
     check_insert(kMultiEchoLaserScanTopic);
   }
 
   // For 3D SLAM, subscribe to all point clouds topics.
-  if (options.num_point_clouds > 0) {
+  if (trajectory_options.num_point_clouds > 0) {
-    for (int i = 0; i < options.num_point_clouds; ++i) {
+    for (int i = 0; i < trajectory_options.num_point_clouds; ++i) {
+      // TODO(hrapp): This code is duplicated in places. Pull out a method.
       string topic = kPointCloud2Topic;
-      if (options.num_point_clouds > 1) {
+      if (trajectory_options.num_point_clouds > 1) {
         topic += "_" + std::to_string(i + 1);
       }
       check_insert(topic);
@@ -130,15 +136,16 @@ void Run(const std::vector<string>& bag_filenames) {
 
   // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
   // required.
-  if (options.map_builder_options.use_trajectory_builder_3d() ||
+  if (node_options.map_builder_options.use_trajectory_builder_3d() ||
-      (options.map_builder_options.use_trajectory_builder_2d() &&
+      (node_options.map_builder_options.use_trajectory_builder_2d() &&
-       options.trajectory_builder_options.trajectory_builder_2d_options()
+       trajectory_options.trajectory_builder_options
+           .trajectory_builder_2d_options()
            .use_imu_data())) {
     check_insert(kImuTopic);
   }
 
   // For both 2D and 3D SLAM, odometry is optional.
-  if (options.use_odometry) {
+  if (trajectory_options.use_odometry) {
     check_insert(kOdometryTopic);
   }
 
@@ -162,7 +169,7 @@ void Run(const std::vector<string>& bag_filenames) {
     }
 
     const int trajectory_id = node.map_builder_bridge()->AddTrajectory(
-        expected_sensor_ids, options.tracking_frame);
+        expected_sensor_ids, trajectory_options);
 
     rosbag::Bag bag;
     bag.open(bag_filename, rosbag::bagmode::Read);

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -0,0 +1,53 @@
+/*
+ * 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_options.h"
+
+#include "glog/logging.h"
+
+namespace cartographer_ros {
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* const
+        lua_parameter_dictionary) {
+  TrajectoryOptions options;
+  options.trajectory_builder_options =
+      ::cartographer::mapping::CreateTrajectoryBuilderOptions(
+          lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
+  options.tracking_frame =
+      lua_parameter_dictionary->GetString("tracking_frame");
+  options.published_frame =
+      lua_parameter_dictionary->GetString("published_frame");
+  options.odom_frame = lua_parameter_dictionary->GetString("odom_frame");
+  options.provide_odom_frame =
+      lua_parameter_dictionary->GetBool("provide_odom_frame");
+  options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
+  options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
+  options.use_multi_echo_laser_scan =
+      lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
+  options.num_point_clouds =
+      lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
+
+  CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
+               (options.num_point_clouds > 0),
+           1)
+      << "Configuration error: 'use_laser_scan', "
+         "'use_multi_echo_laser_scan' and 'num_point_clouds' are "
+         "mutually exclusive, but one is required.";
+
+  return options;
+}
+}  // namespace cartographer_ros

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -0,0 +1,46 @@
+/*
+ * 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_TRAJECTORY_OPTIONS_H_
+#define CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_
+
+#include <string>
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/port.h"
+#include "cartographer_ros/sensor_bridge.h"
+
+namespace cartographer_ros {
+
+struct TrajectoryOptions {
+  ::cartographer::mapping::proto::TrajectoryBuilderOptions
+      trajectory_builder_options;
+  string tracking_frame;
+  string published_frame;
+  string odom_frame;
+  bool provide_odom_frame;
+  bool use_odometry;
+  bool use_laser_scan;
+  bool use_multi_echo_laser_scan;
+  int num_point_clouds;
+};
+
+TrajectoryOptions CreateTrajectoryOptions(
+    ::cartographer::common::LuaParameterDictionary* lua_parameter_dictionary);
+
+}  // namespace cartographer_ros
+
+#endif  // CARTOGRAPHER_ROS_TRAJECTORY_OPTIONS_H_

cartographer_ros_msgs/CMakeLists.txt

@@ -27,12 +27,16 @@ add_message_files(
     SubmapList.msg
     TrajectorySubmapList.msg
     SubmapEntry.msg
+    SensorTopics.msg
+    TrajectoryOptions.msg
 )
 
 add_service_files(
   FILES
     SubmapQuery.srv
     FinishTrajectory.srv
+    StartTrajectory.srv
+    WriteAssets.srv
 )
 
 generate_messages(

cartographer_ros_msgs/msg/SensorTopics.msg

@@ -0,0 +1,19 @@
+# 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.
+
+string laser_scan_topic
+string multi_echo_laser_scan_topic
+string point_cloud2_topic
+string imu_topic
+string odometry_topic

cartographer_ros_msgs/msg/TrajectoryOptions.msg

@@ -0,0 +1,26 @@
+# 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.
+
+string tracking_frame
+string published_frame
+string odom_frame
+bool provide_odom_frame
+bool use_odometry
+bool use_laser_scan
+bool use_multi_echo_laser_scan
+int32 num_point_clouds
+
+# This is a binary-encoded
+# 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.
+string trajectory_builder_options_proto

cartographer_ros_msgs/srv/FinishTrajectory.srv

@@ -12,5 +12,5 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-string stem
+int32 trajectory_id
 ---

cartographer_ros_msgs/srv/StartTrajectory.srv

@@ -0,0 +1,18 @@
+# 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.
+
+cartographer_ros_msgs/TrajectoryOptions options
+cartographer_ros_msgs/SensorTopics topics
+---
+int32 trajectory_id

cartographer_ros_msgs/srv/WriteAssets.srv

@@ -0,0 +1,16 @@
+# 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.
+
+string stem
+---

docs/source/ros_api.rst

@@ -88,11 +88,17 @@ Services
 submap_query (`cartographer_ros_msgs/SubmapQuery`_)
   Fetches the requested submap.
 
+start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)
+  Starts another trajectory by specifying its sensor topics and trajectory
+  options as an binary-encoded proto. Returns an assigned trajectory ID.
+
 finish_trajectory (`cartographer_ros_msgs/FinishTrajectory`_)
-  Finishes the current trajectory by flushing all queued sensor data, running a
+  Finishes the given `trajectory_id`'s trajectory by running a final optimization.
-  final optimization, and writing artifacts (e.g. the map) to disk. The `stem`
+
-  argument is used as a prefix for the various files which are written. Files
+write_assets (`cartographer_ros_msgs/WriteAssets`_)
-  will usually end up in `~/.ros` or `ROS_HOME` if it is set.
+  Writes artifacts (e.g. the map) to disk. The `stem` argument is used as a prefix
+  for the various files which are written. Files will usually end up in `~/.ros` or
+  `ROS_HOME` if it is set.
 
 Required tf Transforms
 ======================