diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 9c62278..ad9388d 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -155,11 +155,11 @@ void Node::AddExtrapolator(const int trajectory_id, void Node::AddSensorSamplers(const int trajectory_id, const TrajectoryOptions& options) { CHECK(sensor_samplers_.count(trajectory_id) == 0); - sensor_samplers_.emplace(std::piecewise_construct, - std::forward_as_tuple(trajectory_id), - std::forward_as_tuple(options.rangefinder_sampling_ratio, - options.odometry_sampling_ratio, - options.imu_sampling_ratio)); + sensor_samplers_.emplace( + std::piecewise_construct, std::forward_as_tuple(trajectory_id), + std::forward_as_tuple(options.rangefinder_sampling_ratio, + options.odometry_sampling_ratio, + options.imu_sampling_ratio)); } void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { @@ -296,24 +296,27 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, for (const string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( - SubscribeWithHandler( - &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, - this)); + {SubscribeWithHandler( + &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, + this), + topic}); } for (const string& topic : ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, options.num_multi_echo_laser_scans)) { subscribers_[trajectory_id].push_back( - SubscribeWithHandler( - &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, - &node_handle_, this)); + {SubscribeWithHandler( + &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, + &node_handle_, this), + topic}); } for (const string& topic : ComputeRepeatedTopicNames( topics.point_cloud2_topic, options.num_point_clouds)) { subscribers_[trajectory_id].push_back( - SubscribeWithHandler( - &Node::HandlePointCloud2Message, trajectory_id, topic, - &node_handle_, this)); + {SubscribeWithHandler( + &Node::HandlePointCloud2Message, trajectory_id, topic, + &node_handle_, this), + topic}); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is @@ -324,17 +327,19 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, .use_imu_data())) { string topic = topics.imu_topic; subscribers_[trajectory_id].push_back( - SubscribeWithHandler(&Node::HandleImuMessage, - trajectory_id, topic, - &node_handle_, this)); + {SubscribeWithHandler(&Node::HandleImuMessage, + trajectory_id, topic, + &node_handle_, this), + topic}); } if (options.use_odometry) { string topic = topics.odometry_topic; subscribers_[trajectory_id].push_back( - SubscribeWithHandler(&Node::HandleOdometryMessage, - trajectory_id, topic, - &node_handle_, this)); + {SubscribeWithHandler(&Node::HandleOdometryMessage, + trajectory_id, topic, + &node_handle_, this), + topic}); } } @@ -421,9 +426,9 @@ bool Node::HandleFinishTrajectory( // Shutdown the subscribers of this trajectory. for (auto& entry : subscribers_[trajectory_id]) { - entry.shutdown(); - subscribed_topics_.erase(entry.getTopic()); - LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]"; + entry.subscriber.shutdown(); + subscribed_topics_.erase(entry.topic); + LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]"; } CHECK_EQ(subscribers_.erase(trajectory_id), 1); map_builder_bridge_.FinishTrajectory(trajectory_id); diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index f4b34b8..b50bbbe 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -94,6 +94,15 @@ class Node { ::ros::NodeHandle* node_handle(); private: + struct Subscriber { + ::ros::Subscriber subscriber; + + // ::ros::Subscriber::getTopic() does not necessarily return the same string + // it was given in its constructor. Since we rely on the topic name as the + // unique identifier of a subscriber, we remember it ourselves. + string topic; + }; + bool HandleSubmapQuery( cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Response& response); @@ -156,7 +165,7 @@ class Node { // These are keyed with 'trajectory_id'. std::map extrapolators_; std::unordered_map sensor_samplers_; - std::unordered_map> subscribers_; + std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_);