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

Fix handling of IMU for 3D SLAM. (#430) 

In the (non-offline) node, subscribing to the IMU topic was
controlled by the 2D options even for 3D SLAM. It now correctly
subscribes always similar to the offline node.
master
Wolfgang Hess 2017-07-20 09:00:16 +02:00 committed by Damon Kohler
parent c58a262d56
commit cf1e2d6cfc
1 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
cartographer_ros/cartographer_ros/node.cc
View File

@ -269,8 +269,12 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
expected_sensor_ids.insert(topic); expected_sensor_ids.insert(topic);
} }
} }
if (options.trajectory_builder_options.trajectory_builder_2d_options() // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
.use_imu_data()) { // required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(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_sensor_ids.insert(topics.imu_topic);
} }
if (options.use_odometry) { if (options.use_odometry) {
@ -328,8 +332,12 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers; point_cloud_subscribers_[trajectory_id] = grouped_point_cloud_subscribers;
} }
if (options.trajectory_builder_options.trajectory_builder_2d_options() // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
.use_imu_data()) { // required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
string topic = topics.imu_topic; string topic = topics.imu_topic;
imu_subscribers_[trajectory_id] = node_handle_.subscribe<sensor_msgs::Imu>( imu_subscribers_[trajectory_id] = node_handle_.subscribe<sensor_msgs::Imu>(
topic, kInfiniteSubscriberQueueSize, topic, kInfiniteSubscriberQueueSize,