12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Insert resolved topic name in subscribed_topics_ (#516) 

As unique id for an individual subscriber.
master
Jihoon Lee 2017-10-09 15:40:53 +02:00 committed by Holger Rapp
parent 5b953d202e
commit f9157b2a26
2 changed files with 38 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
cartographer_ros/cartographer_ros/node.cc
View File

@ -155,11 +155,11 @@ void Node::AddExtrapolator(const int trajectory_id,
void Node::AddSensorSamplers(const int trajectory_id, void Node::AddSensorSamplers(const int trajectory_id,
const TrajectoryOptions& options) { const TrajectoryOptions& options) {
CHECK(sensor_samplers_.count(trajectory_id) == 0); CHECK(sensor_samplers_.count(trajectory_id) == 0);
sensor_samplers_.emplace(std::piecewise_construct, sensor_samplers_.emplace(
std::forward_as_tuple(trajectory_id), std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(options.rangefinder_sampling_ratio, std::forward_as_tuple(options.rangefinder_sampling_ratio,
options.odometry_sampling_ratio, options.odometry_sampling_ratio,
options.imu_sampling_ratio)); options.imu_sampling_ratio));
} }
void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
@ -296,24 +296,27 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
for (const string& topic : ComputeRepeatedTopicNames( for (const string& topic : ComputeRepeatedTopicNames(
topics.laser_scan_topic, options.num_laser_scans)) { topics.laser_scan_topic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
SubscribeWithHandler<sensor_msgs::LaserScan>( {SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_, &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this)); this),
topic});
} }
for (const string& topic : for (const string& topic :
ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic, ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
options.num_multi_echo_laser_scans)) { options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>( {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic, &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this)); &node_handle_, this),
topic});
} }
for (const string& topic : ComputeRepeatedTopicNames( for (const string& topic : ComputeRepeatedTopicNames(
topics.point_cloud2_topic, options.num_point_clouds)) { topics.point_cloud2_topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
SubscribeWithHandler<sensor_msgs::PointCloud2>( {SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic, &Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this)); &node_handle_, this),
topic});
} }
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // 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())) { .use_imu_data())) {
string topic = topics.imu_topic; string topic = topics.imu_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage, {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic, trajectory_id, topic,
&node_handle_, this)); &node_handle_, this),
topic});
} }
if (options.use_odometry) { if (options.use_odometry) {
string topic = topics.odometry_topic; string topic = topics.odometry_topic;
subscribers_[trajectory_id].push_back( subscribers_[trajectory_id].push_back(
SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage, {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, topic, trajectory_id, topic,
&node_handle_, this)); &node_handle_, this),
topic});
} }
} }
@ -421,9 +426,9 @@ bool Node::HandleFinishTrajectory(
// Shutdown the subscribers of this trajectory. // Shutdown the subscribers of this trajectory.
for (auto& entry : subscribers_[trajectory_id]) { for (auto& entry : subscribers_[trajectory_id]) {
entry.shutdown(); entry.subscriber.shutdown();
subscribed_topics_.erase(entry.getTopic()); subscribed_topics_.erase(entry.topic);
LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]"; LOG(INFO) << "Shutdown the subscriber of [" << entry.topic << "]";
} }
CHECK_EQ(subscribers_.erase(trajectory_id), 1); CHECK_EQ(subscribers_.erase(trajectory_id), 1);
map_builder_bridge_.FinishTrajectory(trajectory_id); map_builder_bridge_.FinishTrajectory(trajectory_id);

11
cartographer_ros/cartographer_ros/node.h
View File

@ -94,6 +94,15 @@ class Node {
::ros::NodeHandle* node_handle(); ::ros::NodeHandle* node_handle();
private: 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( bool HandleSubmapQuery(
cartographer_ros_msgs::SubmapQuery::Request& request, cartographer_ros_msgs::SubmapQuery::Request& request,
cartographer_ros_msgs::SubmapQuery::Response& response); cartographer_ros_msgs::SubmapQuery::Response& response);
@ -156,7 +165,7 @@ class Node {
// These are keyed with 'trajectory_id'. // These are keyed with 'trajectory_id'.
std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_; std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_; std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_; std::unordered_map<int, std::vector<Subscriber>> subscribers_;
std::unordered_set<std::string> subscribed_topics_; std::unordered_set<std::string> subscribed_topics_;
std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_); std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);