diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index c14ba87..a6090f5 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -44,6 +44,20 @@ namespace cartographer_ros { +namespace { + +cartographer_ros_msgs::SensorTopics DefaultSensorTopics() { + cartographer_ros_msgs::SensorTopics topics; + topics.laser_scan_topic = kLaserScanTopic; + topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; + topics.point_cloud2_topic = kPointCloud2Topic; + topics.imu_topic = kImuTopic; + topics.odometry_topic = kOdometryTopic; + return topics; +} + +} // namespace + namespace carto = ::cartographer; using carto::transform::Rigid3d; @@ -207,7 +221,7 @@ std::unordered_set Node::ComputeExpectedTopics( const TrajectoryOptions& options, const cartographer_ros_msgs::SensorTopics& topics) { std::unordered_set expected_topics; - + // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. for (const string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans)) { expected_topics.insert(topic); @@ -229,6 +243,7 @@ std::unordered_set Node::ComputeExpectedTopics( .use_imu_data())) { expected_topics.insert(topics.imu_topic); } + // Odometry is optional. if (options.use_odometry) { expected_topics.insert(topics.odometry_topic); } @@ -243,6 +258,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options, map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); + is_active_trajectory_[trajectory_id] = true; subscribed_topics_.insert(expected_sensor_ids.begin(), expected_sensor_ids.end()); return trajectory_id; @@ -328,8 +344,6 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, ->HandleOdometryMessage(topic, msg); }))); } - - is_active_trajectory_[trajectory_id] = true; } bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) { @@ -375,25 +389,29 @@ bool Node::HandleStartTrajectory( LOG(ERROR) << "Invalid topics."; return false; } - - const int trajectory_id = AddTrajectory(options, request.topics); - response.trajectory_id = trajectory_id; - - is_active_trajectory_[trajectory_id] = true; + response.trajectory_id = AddTrajectory(options, request.topics); return true; } void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) { carto::common::MutexLocker lock(&mutex_); - cartographer_ros_msgs::SensorTopics topics; - topics.laser_scan_topic = kLaserScanTopic; - topics.multi_echo_laser_scan_topic = kMultiEchoLaserScanTopic; - topics.point_cloud2_topic = kPointCloud2Topic; - topics.imu_topic = kImuTopic; - topics.odometry_topic = kOdometryTopic; + AddTrajectory(options, DefaultSensorTopics()); +} - const int trajectory_id = AddTrajectory(options, topics); - is_active_trajectory_[trajectory_id] = true; +std::unordered_set Node::ComputeDefaultTopics( + const TrajectoryOptions& options) { + carto::common::MutexLocker lock(&mutex_); + return ComputeExpectedTopics(options, DefaultSensorTopics()); +} + +int Node::AddOfflineTrajectory( + const std::unordered_set& expected_sensor_ids, + const TrajectoryOptions& options) { + carto::common::MutexLocker lock(&mutex_); + const int trajectory_id = + map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); + AddExtrapolator(trajectory_id, options); + return trajectory_id; } bool Node::HandleFinishTrajectory( diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index 8f440fa..6384901 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -57,6 +57,15 @@ class Node { // Starts the first trajectory with the default topics. void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options); + // Compute the default topics for the given 'options'. + std::unordered_set ComputeDefaultTopics( + const TrajectoryOptions& options); + + // Adds a trajectory for offline processing, i.e. not listening to topics. + int AddOfflineTrajectory( + const std::unordered_set& expected_sensor_ids, + const TrajectoryOptions& options); + // Loads a persisted state to use as a map. void LoadMap(const std::string& map_filename); diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index 4034ab2..374873b 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -110,39 +110,9 @@ void Run(const std::vector& bag_filenames) { } std::unordered_set expected_sensor_ids; - const auto check_insert = [&expected_sensor_ids, &node](const string& topic) { + for (const string& topic : node.ComputeDefaultTopics(trajectory_options)) { CHECK(expected_sensor_ids.insert(node.node_handle()->resolveName(topic)) .second); - }; - - // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. - for (const string& topic : ComputeRepeatedTopicNames( - kLaserScanTopic, trajectory_options.num_laser_scans)) { - check_insert(topic); - } - for (const string& topic : ComputeRepeatedTopicNames( - kMultiEchoLaserScanTopic, - trajectory_options.num_multi_echo_laser_scans)) { - check_insert(topic); - } - 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 - // required. - if (node_options.map_builder_options.use_trajectory_builder_3d() || - (node_options.map_builder_options.use_trajectory_builder_2d() && - trajectory_options.trajectory_builder_options - .trajectory_builder_2d_options() - .use_imu_data())) { - check_insert(kImuTopic); - } - - // Odometry is optional. - if (trajectory_options.use_odometry) { - check_insert(kOdometryTopic); } ::ros::Publisher tf_publisher = @@ -164,8 +134,8 @@ void Run(const std::vector& bag_filenames) { break; } - const int trajectory_id = node.map_builder_bridge()->AddTrajectory( - expected_sensor_ids, trajectory_options); + const int trajectory_id = + node.AddOfflineTrajectory(expected_sensor_ids, trajectory_options); rosbag::Bag bag; bag.open(bag_filename, rosbag::bagmode::Read);