From 63aba81e31c3c1ba250e93f37210598febf8cc3a Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 1 Aug 2017 18:16:21 +0200 Subject: [PATCH] Reuse the Node::Handle*() functions. (#466) This moves adding IMU data to the extrapolator to Node::HandleImuMessage() so that in the offline node extrapolation for tf publishing can benefit from IMU data. Fixes crash in offline node by setting the trajectories as active as they are added. --- cartographer_ros/cartographer_ros/node.cc | 94 ++++++++++------------- 1 file changed, 41 insertions(+), 53 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index e65797d..e75b92b 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -56,6 +56,23 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { return topics; } +// Subscribes to the 'topic' for 'trajectory_id' using the 'node_handle' and +// calls 'handler' on the 'node' to handle messages. Returns the subscriber. +template +::ros::Subscriber SubscribeWithHandler( + void (Node::*handler)(int, const string&, + const typename MessageType::ConstPtr&), + const int trajectory_id, const string& topic, + ::ros::NodeHandle* const node_handle, Node* const node) { + return node_handle->subscribe( + topic, kInfiniteSubscriberQueueSize, + boost::function( + [node, handler, trajectory_id, + topic](const typename MessageType::ConstPtr& msg) { + (node->*handler)(trajectory_id, topic, msg); + })); +} + } // namespace namespace carto = ::cartographer; @@ -268,41 +285,24 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, for (const string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( - node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::LaserScan::ConstPtr& msg) { - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleLaserScanMessage(topic, msg); - }))); + SubscribeWithHandler( + &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, + this)); } for (const string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { - subscribers_[trajectory_id].push_back(node_handle_.subscribe< - sensor_msgs::MultiEchoLaserScan>( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage(topic, msg); - }))); + subscribers_[trajectory_id].push_back( + SubscribeWithHandler( + &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, + &node_handle_, this)); } for (const string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { - subscribers_[trajectory_id].push_back(node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandlePointCloud2Message(topic, msg); - }))); + subscribers_[trajectory_id].push_back( + SubscribeWithHandler( + &Node::HandlePointCloud2Message, trajectory_id, topic, + &node_handle_, this)); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is @@ -313,34 +313,17 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, .use_imu_data())) { string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( - node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::Imu::ConstPtr& msg) { - carto::common::MutexLocker lock(&mutex_); - auto sensor_bridge_ptr = - map_builder_bridge_.sensor_bridge(trajectory_id); - auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); - if (imu_data_ptr != nullptr) { - extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); - } - sensor_bridge_ptr->HandleImuMessage(topic, msg); - }))); + SubscribeWithHandler(&Node::HandleImuMessage, + trajectory_id, topic, + &node_handle_, this)); } if (options.use_odometry) { string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( - node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const nav_msgs::Odometry::ConstPtr& msg) { - carto::common::MutexLocker lock(&mutex_); - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleOdometryMessage(topic, msg); - }))); + SubscribeWithHandler(&Node::HandleOdometryMessage, + trajectory_id, topic, + &node_handle_, this)); } } @@ -405,6 +388,7 @@ int Node::AddOfflineTrajectory( const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); + is_active_trajectory_[trajectory_id] = true; return trajectory_id; } @@ -471,8 +455,12 @@ void Node::HandleOdometryMessage(const int trajectory_id, 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); + auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); + auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); + if (imu_data_ptr != nullptr) { + extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr); + } + sensor_bridge_ptr->HandleImuMessage(sensor_id, msg); } void Node::HandleLaserScanMessage(const int trajectory_id,