From d60e1e2e119eaea0f4387e9197fd9a3e852a75da Mon Sep 17 00:00:00 2001
From: Wolfgang Hess <whess@google.com>
Date: Wed, 26 Jul 2017 16:08:48 +0200
Subject: [PATCH] Use extrapolators to provide higher frequency tf. (#451)

Fixes googlecartographer/cartographer#102 for Cartographer ROS.
Direct users of the Cartographer library can make use of
googlecartographer/cartographer#430.
---
 cartographer_ros/cartographer_ros/node.cc | 19 +++++++++++++++----
 cartographer_ros/cartographer_ros/node.h  |  5 +++--
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc
index ea9d1f6..9b73139 100644
--- a/cartographer_ros/cartographer_ros/node.cc
+++ b/cartographer_ros/cartographer_ros/node.cc
@@ -111,24 +111,35 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
   for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
     const auto& trajectory_state = entry.second;
 
+    // TODO(whess): Use improved models, e.g. using odometry and IMU when
+    // available.
+    constexpr double extrapolation_estimation_time_sec = 0.001;  // 1 ms
+    if (extrapolators_.count(entry.first) == 0) {
+      extrapolators_.emplace(
+          std::piecewise_construct, std::forward_as_tuple(entry.first),
+          std::forward_as_tuple(::cartographer::common::FromSeconds(
+              extrapolation_estimation_time_sec)));
+    }
+    auto& extrapolator = extrapolators_.at(entry.first);
     // We only publish a point cloud if it has changed. It is not needed at high
     // frequency, and republishing it would be computationally wasteful.
-    if (trajectory_state.pose_estimate.time !=
-        last_scan_matched_point_cloud_time_) {
+    if (trajectory_state.pose_estimate.time != extrapolator.GetLastPoseTime()) {
       scan_matched_point_cloud_publisher_.publish(ToPointCloud2Message(
           carto::common::ToUniversal(trajectory_state.pose_estimate.time),
           node_options_.map_frame,
           carto::sensor::TransformPointCloud(
               trajectory_state.pose_estimate.point_cloud,
               trajectory_state.local_to_map.cast<float>())));
-      last_scan_matched_point_cloud_time_ = trajectory_state.pose_estimate.time;
+      extrapolator.AddPose(trajectory_state.pose_estimate.time,
+                           trajectory_state.pose_estimate.pose);
     }
 
     geometry_msgs::TransformStamped stamped_transform;
     // If we do not publish a new point cloud, we still allow time of the
     // published poses to advance.
     stamped_transform.header.stamp = ros::Time::now();
-    const auto& tracking_to_local = trajectory_state.pose_estimate.pose;
+    const Rigid3d tracking_to_local =
+        extrapolator.ExtrapolatePose(FromRos(stamped_transform.header.stamp));
     const Rigid3d tracking_to_map =
         trajectory_state.local_to_map * tracking_to_local;
 
diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h
index 81bb7fd..38395dc 100644
--- a/cartographer_ros/cartographer_ros/node.h
+++ b/cartographer_ros/cartographer_ros/node.h
@@ -17,12 +17,14 @@
 #ifndef CARTOGRAPHER_ROS_NODE_H_
 #define CARTOGRAPHER_ROS_NODE_H_
 
+#include <map>
 #include <memory>
 #include <unordered_map>
 #include <unordered_set>
 #include <vector>
 
 #include "cartographer/common/mutex.h"
+#include "cartographer/mapping/pose_extrapolator.h"
 #include "cartographer_ros/map_builder_bridge.h"
 #include "cartographer_ros/node_constants.h"
 #include "cartographer_ros/node_options.h"
@@ -105,10 +107,9 @@ class Node {
   // These ros::ServiceServers need to live for the lifetime of the node.
   std::vector<::ros::ServiceServer> service_servers_;
   ::ros::Publisher scan_matched_point_cloud_publisher_;
-  cartographer::common::Time last_scan_matched_point_cloud_time_ =
-      cartographer::common::Time::min();
 
   // These are keyed with 'trajectory_id'.
+  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
   std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
   std::unordered_set<std::string> subscribed_topics_;
   std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);