From ace7ab5f0549d2ec50dc5a96f6e4a3f010b434a1 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 1 Aug 2017 13:38:34 +0200 Subject: [PATCH] Crash fix in offline node. (#464) This adds trajectories at the Node object. It makes sure all necessary extrapolators exist. Before the offline node would crash when extrapolating. Also deduplicates the logic to compute the topics for a trajectory. --- cartographer_ros/cartographer_ros/node.cc | 50 +++++++++++++------ cartographer_ros/cartographer_ros/node.h | 9 ++++ .../cartographer_ros/offline_node_main.cc | 36 ++----------- 3 files changed, 46 insertions(+), 49 deletions(-) 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);