diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 5a25b54..ed429ce 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -48,33 +48,6 @@ namespace carto = ::cartographer; using carto::transform::Rigid3d; -namespace { - -void ShutdownSubscriber(std::unordered_map& subscribers, - int trajectory_id) { - if (subscribers.count(trajectory_id) == 0) { - return; - } - subscribers[trajectory_id].shutdown(); - LOG(INFO) << "Shutdown the subscriber of [" - << subscribers[trajectory_id].getTopic() << "]"; - CHECK_EQ(subscribers.erase(trajectory_id), 1); -} - -bool IsTopicNameUnique( - const string& topic, - const std::unordered_map& subscribers) { - for (auto& entry : subscribers) { - if (entry.second.getTopic() == topic) { - LOG(ERROR) << "Topic name [" << topic << "] is already used."; - return false; - } - } - return true; -} - -} // namespace - Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer) : node_options_(node_options), map_builder_bridge_(node_options_, tf_buffer) { @@ -247,24 +220,20 @@ void Node::SpinOccupancyGridThreadForever() { } } -int Node::AddTrajectory(const TrajectoryOptions& options, - const cartographer_ros_msgs::SensorTopics& topics) { - std::unordered_set expected_sensor_ids; +std::unordered_set Node::ComputeExpectedTopics( + const TrajectoryOptions& options, + const cartographer_ros_msgs::SensorTopics& topics) { + std::unordered_set expected_topics; if (options.use_laser_scan) { - expected_sensor_ids.insert(topics.laser_scan_topic); + expected_topics.insert(topics.laser_scan_topic); } if (options.use_multi_echo_laser_scan) { - expected_sensor_ids.insert(topics.multi_echo_laser_scan_topic); + expected_topics.insert(topics.multi_echo_laser_scan_topic); } - if (options.num_point_clouds > 0) { - for (int i = 0; i < options.num_point_clouds; ++i) { - string topic = topics.point_cloud2_topic; - if (options.num_point_clouds > 1) { - topic += "_" + std::to_string(i + 1); - } - expected_sensor_ids.insert(topic); - } + for (const string& topic : ComputeRepeatedTopicNames( + topics.point_cloud2_topic, options.num_point_clouds)) { + expected_topics.insert(topic); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is // required. @@ -272,12 +241,24 @@ int Node::AddTrajectory(const TrajectoryOptions& options, (node_options_.map_builder_options.use_trajectory_builder_2d() && options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { - expected_sensor_ids.insert(topics.imu_topic); + expected_topics.insert(topics.imu_topic); } if (options.use_odometry) { - expected_sensor_ids.insert(topics.odometry_topic); + expected_topics.insert(topics.odometry_topic); } - return map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + return expected_topics; +} + +int Node::AddTrajectory(const TrajectoryOptions& options, + const cartographer_ros_msgs::SensorTopics& topics) { + const std::unordered_set expected_sensor_ids = + ComputeExpectedTopics(options, topics); + const int trajectory_id = + map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + LaunchSubscribers(options, topics, trajectory_id); + subscribed_topics_.insert(expected_sensor_ids.begin(), + expected_sensor_ids.end()); + return trajectory_id; } void Node::LaunchSubscribers(const TrajectoryOptions& options, @@ -285,7 +266,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, const int trajectory_id) { if (options.use_laser_scan) { const string topic = topics.laser_scan_topic; - laser_scan_subscribers_[trajectory_id] = + subscribers_[trajectory_id].push_back( node_handle_.subscribe( topic, kInfiniteSubscriberQueueSize, boost::function( @@ -293,40 +274,32 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, topic](const sensor_msgs::LaserScan::ConstPtr& msg) { map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleLaserScanMessage(topic, msg); - })); + }))); } if (options.use_multi_echo_laser_scan) { const string topic = topics.multi_echo_laser_scan_topic; - multi_echo_laser_scan_subscribers_[trajectory_id] = - node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleMultiEchoLaserScanMessage(topic, msg); - })); + 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) { + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleMultiEchoLaserScanMessage(topic, msg); + }))); } - std::vector<::ros::Subscriber> grouped_point_cloud_subscribers; - if (options.num_point_clouds > 0) { - for (int i = 0; i < options.num_point_clouds; ++i) { - string topic = topics.point_cloud2_topic; - if (options.num_point_clouds > 1) { - topic += "_" + std::to_string(i + 1); - } - grouped_point_cloud_subscribers.push_back(node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::PointCloud2::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandlePointCloud2Message(topic, msg); - }))); - } - point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers; + 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) { + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandlePointCloud2Message(topic, msg); + }))); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is @@ -336,19 +309,20 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, options.trajectory_builder_options.trajectory_builder_2d_options() .use_imu_data())) { string topic = topics.imu_topic; - imu_subscribers_[trajectory_id] = node_handle_.subscribe( - topic, kInfiniteSubscriberQueueSize, - boost::function( - [this, trajectory_id, - topic](const sensor_msgs::Imu::ConstPtr& msg) { - map_builder_bridge_.sensor_bridge(trajectory_id) - ->HandleImuMessage(topic, msg); - })); + subscribers_[trajectory_id].push_back( + node_handle_.subscribe( + topic, kInfiniteSubscriberQueueSize, + boost::function( + [this, trajectory_id, + topic](const sensor_msgs::Imu::ConstPtr& msg) { + map_builder_bridge_.sensor_bridge(trajectory_id) + ->HandleImuMessage(topic, msg); + }))); } if (options.use_odometry) { string topic = topics.odometry_topic; - odom_subscribers_[trajectory_id] = + subscribers_[trajectory_id].push_back( node_handle_.subscribe( topic, kInfiniteSubscriberQueueSize, boost::function( @@ -356,7 +330,7 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, topic](const nav_msgs::Odometry::ConstPtr& msg) { map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleOdometryMessage(topic, msg); - })); + }))); } is_active_trajectory_[trajectory_id] = true; @@ -379,34 +353,13 @@ bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { return false; } -bool Node::ValidateTopicName( +bool Node::ValidateTopicNames( const ::cartographer_ros_msgs::SensorTopics& topics, const TrajectoryOptions& options) { - if (!IsTopicNameUnique(topics.laser_scan_topic, laser_scan_subscribers_)) { - return false; - } - if (!IsTopicNameUnique(topics.multi_echo_laser_scan_topic, - multi_echo_laser_scan_subscribers_)) { - return false; - } - if (!IsTopicNameUnique(topics.imu_topic, imu_subscribers_)) { - return false; - } - if (!IsTopicNameUnique(topics.odometry_topic, odom_subscribers_)) { - return false; - } - for (auto& subscribers : point_cloud_subscribers_) { - string topic = topics.point_cloud2_topic; - int count = 0; - for (auto& subscriber : subscribers.second) { - if (options.num_point_clouds > 1) { - topic += "_" + std::to_string(count + 1); - ++count; - } - if (subscriber.getTopic() == topic) { - LOG(ERROR) << "Topic name [" << topic << "] is already used"; - return false; - } + for (const std::string& topic : ComputeExpectedTopics(options, topics)) { + if (subscribed_topics_.count(topic) > 0) { + LOG(ERROR) << "Topic name [" << topic << "] is already used."; + return false; } } return true; @@ -422,13 +375,12 @@ bool Node::HandleStartTrajectory( LOG(ERROR) << "Invalid trajectory options."; return false; } - if (!Node::ValidateTopicName(request.topics, options)) { + if (!Node::ValidateTopicNames(request.topics, options)) { LOG(ERROR) << "Invalid topics."; return false; } const int trajectory_id = AddTrajectory(options, request.topics); - LaunchSubscribers(options, request.topics, trajectory_id); response.trajectory_id = trajectory_id; is_active_trajectory_[trajectory_id] = true; @@ -445,7 +397,6 @@ void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { topics.odometry_topic = kOdometryTopic; const int trajectory_id = AddTrajectory(options, topics); - LaunchSubscribers(options, topics, trajectory_id); is_active_trajectory_[trajectory_id] = true; } @@ -464,18 +415,13 @@ bool Node::HandleFinishTrajectory( return false; } - ShutdownSubscriber(laser_scan_subscribers_, trajectory_id); - ShutdownSubscriber(multi_echo_laser_scan_subscribers_, trajectory_id); - ShutdownSubscriber(odom_subscribers_, trajectory_id); - ShutdownSubscriber(imu_subscribers_, trajectory_id); - - if (point_cloud_subscribers_.count(trajectory_id) != 0) { - for (auto& entry : point_cloud_subscribers_[trajectory_id]) { - LOG(INFO) << "Shutdown the subscriber of [" << entry.getTopic() << "]"; - entry.shutdown(); - } - CHECK_EQ(point_cloud_subscribers_.erase(trajectory_id), 1); + // 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() << "]"; } + CHECK_EQ(subscribers_.erase(trajectory_id), 1); map_builder_bridge_.FinishTrajectory(trajectory_id); is_active_trajectory_[trajectory_id] = false; return true; diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 31b6570..eaa9e68 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -18,6 +18,8 @@ #define CARTOGRAPHER_ROS_NODE_H_ #include +#include +#include #include #include "cartographer/common/mutex.h" @@ -72,6 +74,10 @@ class Node { bool HandleWriteAssets( cartographer_ros_msgs::WriteAssets::Request& request, cartographer_ros_msgs::WriteAssets::Response& response); + // Returns the set of topic names we want to subscribe to. + std::unordered_set ComputeExpectedTopics( + const TrajectoryOptions& options, + const cartographer_ros_msgs::SensorTopics& topics); int AddTrajectory(const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics); void LaunchSubscribers(const TrajectoryOptions& options, @@ -83,8 +89,8 @@ class Node { void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); void SpinOccupancyGridThreadForever(); bool ValidateTrajectoryOptions(const TrajectoryOptions& options); - bool ValidateTopicName(const ::cartographer_ros_msgs::SensorTopics& topics, - const TrajectoryOptions& options); + bool ValidateTopicNames(const ::cartographer_ros_msgs::SensorTopics& topics, + const TrajectoryOptions& options); const NodeOptions node_options_; @@ -104,12 +110,8 @@ class Node { cartographer::common::Time::min(); // These are keyed with 'trajectory_id'. - std::unordered_map laser_scan_subscribers_; - std::unordered_map multi_echo_laser_scan_subscribers_; - std::unordered_map odom_subscribers_; - std::unordered_map imu_subscribers_; - std::unordered_map> - point_cloud_subscribers_; + std::unordered_map> subscribers_; + std::unordered_set subscribed_topics_; std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_); ::ros::Publisher occupancy_grid_publisher_; std::thread occupancy_grid_thread_; diff --git a/cartographer_ros/cartographer_ros/node_constants.cc b/cartographer_ros/cartographer_ros/node_constants.cc new file mode 100644 index 0000000..415ad37 --- /dev/null +++ b/cartographer_ros/cartographer_ros/node_constants.cc @@ -0,0 +1,37 @@ +/* + * Copyright 2016 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer_ros/node_constants.h" + +#include "glog/logging.h" + +namespace cartographer_ros { + +std::vector ComputeRepeatedTopicNames(const std::string& topic, + const int num_topics) { + CHECK_GE(num_topics, 0); + if (num_topics == 1) { + return {topic}; + } + std::vector topics; + topics.reserve(num_topics); + for (int i = 0; i < num_topics; ++i) { + topics.emplace_back(topic + "_" + std::to_string(i + 1)); + } + return topics; +} + +} // namespace cartographer_ros diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index d3b72fd..c861c96 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/cartographer_ros/cartographer_ros/node_constants.h @@ -17,6 +17,9 @@ #ifndef CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ #define CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ +#include +#include + namespace cartographer_ros { // Default topic names; expected to be remapped as needed. @@ -39,6 +42,10 @@ constexpr double kConstraintPublishPeriodSec = 0.5; constexpr int kInfiniteSubscriberQueueSize = 0; constexpr int kLatestOnlyPublisherQueueSize = 1; +// For multiple topics adds numbers to the topic name and returns the list. +std::vector ComputeRepeatedTopicNames(const std::string& topic, + int num_topics); + } // namespace cartographer_ros #endif // CARTOGRAPHER_ROS_NODE_CONSTANTS_H_ diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 148a078..a4f24f6 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -123,16 +123,10 @@ void Run(const std::vector& bag_filenames) { check_insert(kMultiEchoLaserScanTopic); } - // For 3D SLAM, subscribe to all point clouds topics. - if (trajectory_options.num_point_clouds > 0) { - for (int i = 0; i < trajectory_options.num_point_clouds; ++i) { - // TODO(hrapp): This code is duplicated in places. Pull out a method. - string topic = kPointCloud2Topic; - if (trajectory_options.num_point_clouds > 1) { - topic += "_" + std::to_string(i + 1); - } - check_insert(topic); - } + // Subscribe to all point cloud topics. + for (const string& topic : ComputeRepeatedTopicNames( + kPointCloud2Topic, trajectory_options.num_point_clouds)) { + check_insert(topic); } // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is