From 2df3b83c80655780f1c6f371ec4be8f799ceb9ee Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 19 Jul 2018 18:36:13 +0200 Subject: [PATCH] Add option to disable PoseExtrapolator (#946) This is useful for tuning/debugging to rule out (simulated) time issues (because published pose will then only depend on header times). Another use case is when Cartographer runs on a separate machine that has a different system clock than the sensors. --- cartographer_ros/cartographer_ros/node.cc | 14 ++++++++++---- cartographer_ros/cartographer_ros/node_options.cc | 4 ++++ cartographer_ros/cartographer_ros/node_options.h | 1 + .../configuration_files/backpack_2d.lua | 1 + .../configuration_files/backpack_3d.lua | 1 + cartographer_ros/configuration_files/pr2.lua | 1 + cartographer_ros/configuration_files/revo_lds.lua | 1 + .../configuration_files/taurob_tracker.lua | 1 + 8 files changed, 20 insertions(+), 4 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index bdaf5c4..430b33c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -231,14 +231,20 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) { // information is better. const ::cartographer::common::Time now = std::max( FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime()); - stamped_transform.header.stamp = ToRos(now); - + stamped_transform.header.stamp = + node_options_.use_pose_extrapolator + ? ToRos(now) + : ToRos(trajectory_data.local_slam_data->time); + const Rigid3d tracking_to_local_3d = + node_options_.use_pose_extrapolator + ? extrapolator.ExtrapolatePose(now) + : trajectory_data.local_slam_data->local_pose; const Rigid3d tracking_to_local = [&] { if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) { return carto::transform::Embed3D( - carto::transform::Project2D(extrapolator.ExtrapolatePose(now))); + carto::transform::Project2D(tracking_to_local_3d)); } - return extrapolator.ExtrapolatePose(now); + return tracking_to_local_3d; }(); const Rigid3d tracking_to_map = diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 004006c..42acde3 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/cartographer_ros/cartographer_ros/node_options.cc @@ -40,6 +40,10 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetDouble("pose_publish_period_sec"); options.trajectory_publish_period_sec = lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec"); + if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) { + options.use_pose_extrapolator = + lua_parameter_dictionary->GetBool("use_pose_extrapolator"); + } return options; } diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 7acfbba..3021581 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/cartographer_ros/cartographer_ros/node_options.h @@ -35,6 +35,7 @@ struct NodeOptions { double submap_publish_period_sec; double pose_publish_period_sec; double trajectory_publish_period_sec; + bool use_pose_extrapolator = true; }; NodeOptions CreateNodeOptions( diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 85699db..8d29069 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index ad730ba..b5c05f3 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 31ec614..ed6d473 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index f6935e9..c9f5ab0 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = true, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = false, use_nav_sat = false, use_landmarks = false, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index d1cd4c3..1097018 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -24,6 +24,7 @@ options = { odom_frame = "odom", provide_odom_frame = false, publish_frame_projected_to_2d = false, + use_pose_extrapolator = true, use_odometry = true, use_nav_sat = false, use_landmarks = false,