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

@@ -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 =

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;
 }
 

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(

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,

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,

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,

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,

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,