From 76b1903488b870c6b7bdaa0ed960c618e9c62d9f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Tue, 1 Aug 2017 17:07:58 +0200 Subject: [PATCH] Make Node thread-safe. (#443) Accessing the MapBuilderBridge was not previously guarded by a mutex. --- cartographer_ros/cartographer_ros/node.cc | 53 ++++++++++++++++++- cartographer_ros/cartographer_ros/node.h | 19 ++++++- .../cartographer_ros/offline_node_main.cc | 40 ++++++-------- 3 files changed, 85 insertions(+), 27 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 869ca25..e65797d 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 6384901..7f01d20 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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& 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( diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 374873b..a59a35a 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -173,35 +173,28 @@ void Run(const std::vector& bag_filenames) { const string topic = node.node_handle()->resolveName( delayed_msg.getTopic(), false /* resolve */); if (delayed_msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleLaserScanMessage( - topic, delayed_msg.instantiate()); + node.HandleLaserScanMessage( + trajectory_id, topic, + delayed_msg.instantiate()); } if (delayed_msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage( - topic, - delayed_msg.instantiate()); + node.HandleMultiEchoLaserScanMessage( + trajectory_id, topic, + delayed_msg.instantiate()); } if (delayed_msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandlePointCloud2Message( - topic, delayed_msg.instantiate()); + node.HandlePointCloud2Message( + trajectory_id, topic, + delayed_msg.instantiate()); } if (delayed_msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleImuMessage(topic, - delayed_msg.instantiate()); + node.HandleImuMessage(trajectory_id, topic, + delayed_msg.instantiate()); } if (delayed_msg.isType()) { - node.map_builder_bridge() - ->sensor_bridge(trajectory_id) - ->HandleOdometryMessage( - topic, delayed_msg.instantiate()); + node.HandleOdometryMessage( + trajectory_id, topic, + delayed_msg.instantiate()); } rosgraph_msgs::Clock clock; clock.clock = delayed_msg.getTime(); @@ -225,7 +218,7 @@ void Run(const std::vector& bag_filenames) { } bag.close(); - node.map_builder_bridge()->FinishTrajectory(trajectory_id); + node.FinishTrajectory(trajectory_id); } const std::chrono::time_point end_time = @@ -243,8 +236,7 @@ void Run(const std::vector& 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