Make Node thread-safe. (#443)

Accessing the MapBuilderBridge was not previously guarded by a mutex.

cartographer_ros/cartographer_ros/node.cc

@@ -106,8 +106,6 @@ Node::~Node() {}
 
 ::ros::NodeHandle* Node::node_handle() { return &node_handle_; }
 
-MapBuilderBridge* Node::map_builder_bridge() { return &map_builder_bridge_; }
-
 bool Node::HandleSubmapQuery(
     ::cartographer_ros_msgs::SubmapQuery::Request& request,
     ::cartographer_ros_msgs::SubmapQuery::Response& response) {
@@ -455,6 +453,57 @@ void Node::FinishAllTrajectories() {
   }
 }
 
+void Node::FinishTrajectory(const int trajectory_id) {
+  carto::common::MutexLocker lock(&mutex_);
+  CHECK(is_active_trajectory_.at(trajectory_id));
+  map_builder_bridge_.FinishTrajectory(trajectory_id);
+  is_active_trajectory_[trajectory_id] = false;
+}
+
+void Node::HandleOdometryMessage(const int trajectory_id,
+                                 const string& sensor_id,
+                                 const nav_msgs::Odometry::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleOdometryMessage(sensor_id, msg);
+}
+
+void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id,
+                            const sensor_msgs::Imu::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleImuMessage(sensor_id, msg);
+}
+
+void Node::HandleLaserScanMessage(const int trajectory_id,
+                                  const string& sensor_id,
+                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleLaserScanMessage(sensor_id, msg);
+}
+
+void Node::HandleMultiEchoLaserScanMessage(
+    int trajectory_id, const string& sensor_id,
+    const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
+}
+
+void Node::HandlePointCloud2Message(
+    const int trajectory_id, const string& sensor_id,
+    const sensor_msgs::PointCloud2::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandlePointCloud2Message(sensor_id, msg);
+}
+
+void Node::SerializeState(const string& filename) {
+  carto::common::MutexLocker lock(&mutex_);
+  map_builder_bridge_.SerializeState(filename);
+}
+
 void Node::LoadMap(const std::string& map_filename) {
   carto::common::MutexLocker lock(&mutex_);
   map_builder_bridge_.LoadMap(map_filename);

cartographer_ros/cartographer_ros/node.h

@@ -53,6 +53,8 @@ class Node {
 
   // Finishes all yet active trajectories.
   void FinishAllTrajectories();
+  // Finishes a single given trajectory.
+  void FinishTrajectory(int trajectory_id);
 
   // Starts the first trajectory with the default topics.
   void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);
@@ -66,11 +68,26 @@ class Node {
       const std::unordered_set<string>& expected_sensor_ids,
       const TrajectoryOptions& options);
 
+  // The following functions handle adding sensor data to a trajectory.
+  void HandleOdometryMessage(int trajectory_id, const string& sensor_id,
+                             const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleImuMessage(int trajectory_id, const string& sensor_id,
+                        const sensor_msgs::Imu::ConstPtr& msg);
+  void HandleLaserScanMessage(int trajectory_id, const string& sensor_id,
+                              const sensor_msgs::LaserScan::ConstPtr& msg);
+  void HandleMultiEchoLaserScanMessage(
+      int trajectory_id, const string& sensor_id,
+      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
+  void HandlePointCloud2Message(int trajectory_id, const string& sensor_id,
+                                const sensor_msgs::PointCloud2::ConstPtr& msg);
+
+  // Serializes the complete Node state.
+  void SerializeState(const string& filename);
+
   // Loads a persisted state to use as a map.
   void LoadMap(const std::string& map_filename);
 
   ::ros::NodeHandle* node_handle();
-  MapBuilderBridge* map_builder_bridge();
 
  private:
   bool HandleSubmapQuery(

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -173,35 +173,28 @@ void Run(const std::vector<string>& bag_filenames) {
         const string topic = node.node_handle()->resolveName(
             delayed_msg.getTopic(), false /* resolve */);
         if (delayed_msg.isType<sensor_msgs::LaserScan>()) {
-          node.map_builder_bridge()
-              ->sensor_bridge(trajectory_id)
-              ->HandleLaserScanMessage(
-                  topic, delayed_msg.instantiate<sensor_msgs::LaserScan>());
+          node.HandleLaserScanMessage(
+              trajectory_id, topic,
+              delayed_msg.instantiate<sensor_msgs::LaserScan>());
         }
         if (delayed_msg.isType<sensor_msgs::MultiEchoLaserScan>()) {
-          node.map_builder_bridge()
-              ->sensor_bridge(trajectory_id)
-              ->HandleMultiEchoLaserScanMessage(
-                  topic,
-                  delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
+          node.HandleMultiEchoLaserScanMessage(
+              trajectory_id, topic,
+              delayed_msg.instantiate<sensor_msgs::MultiEchoLaserScan>());
         }
         if (delayed_msg.isType<sensor_msgs::PointCloud2>()) {
-          node.map_builder_bridge()
-              ->sensor_bridge(trajectory_id)
-              ->HandlePointCloud2Message(
-                  topic, delayed_msg.instantiate<sensor_msgs::PointCloud2>());
+          node.HandlePointCloud2Message(
+              trajectory_id, topic,
+              delayed_msg.instantiate<sensor_msgs::PointCloud2>());
         }
         if (delayed_msg.isType<sensor_msgs::Imu>()) {
-          node.map_builder_bridge()
-              ->sensor_bridge(trajectory_id)
-              ->HandleImuMessage(topic,
-                                 delayed_msg.instantiate<sensor_msgs::Imu>());
+          node.HandleImuMessage(trajectory_id, topic,
+                                delayed_msg.instantiate<sensor_msgs::Imu>());
         }
         if (delayed_msg.isType<nav_msgs::Odometry>()) {
-          node.map_builder_bridge()
-              ->sensor_bridge(trajectory_id)
-              ->HandleOdometryMessage(
-                  topic, delayed_msg.instantiate<nav_msgs::Odometry>());
+          node.HandleOdometryMessage(
+              trajectory_id, topic,
+              delayed_msg.instantiate<nav_msgs::Odometry>());
         }
         rosgraph_msgs::Clock clock;
         clock.clock = delayed_msg.getTime();
@@ -225,7 +218,7 @@ void Run(const std::vector<string>& bag_filenames) {
     }
 
     bag.close();
-    node.map_builder_bridge()->FinishTrajectory(trajectory_id);
+    node.FinishTrajectory(trajectory_id);
   }
 
   const std::chrono::time_point<std::chrono::steady_clock> end_time =
@@ -243,8 +236,7 @@ void Run(const std::vector<string>& bag_filenames) {
             << (cpu_timespec.tv_sec + 1e-9 * cpu_timespec.tv_nsec) << " s";
 #endif
 
-  node.map_builder_bridge()->SerializeState(bag_filenames.front() +
-                                            ".pbstream");
+  node.SerializeState(bag_filenames.front() + ".pbstream");
 }
 
 }  // namespace