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,
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<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this));
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&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<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this));
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&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<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this));
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
&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<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic,
&node_handle_, this));
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, topic,
&node_handle_, this),
topic});
}
if (options.use_odometry) {
string topic = topics.odometry_topic;
subscribers_[trajectory_id].push_back(
SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, topic,
&node_handle_, this));
{SubscribeWithHandler<nav_msgs::Odometry>(&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);

11
cartographer_ros/cartographer_ros/node.h
View File

@ -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<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
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_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);