Also extrapolate based on IMU if available. (#458)

cartographer_ros/cartographer_ros/node.cc

@@ -106,21 +106,24 @@ void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
   submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
 }
 
+::cartographer::mapping::PoseExtrapolator* Node::GetExtrapolator(
+    int trajectory_id) {
+  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
+  if (extrapolators_.count(trajectory_id) == 0) {
+    extrapolators_.emplace(
+        std::piecewise_construct, std::forward_as_tuple(trajectory_id),
+        std::forward_as_tuple(::cartographer::common::FromSeconds(
+            kExtrapolationEstimationTimeSec)));
+  }
+  return &extrapolators_.at(trajectory_id);
+}
+
 void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
   carto::common::MutexLocker lock(&mutex_);
   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);
+    auto& extrapolator = *GetExtrapolator(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 != extrapolator.GetLastPoseTime()) {
@@ -292,8 +295,13 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
             boost::function<void(const sensor_msgs::Imu::ConstPtr&)>(
                 [this, trajectory_id,
                  topic](const sensor_msgs::Imu::ConstPtr& msg) {
-                  map_builder_bridge_.sensor_bridge(trajectory_id)
-                      ->HandleImuMessage(topic, msg);
+                  auto sensor_bridge_ptr =
+                      map_builder_bridge_.sensor_bridge(trajectory_id);
+                  sensor_bridge_ptr->HandleImuMessage(topic, msg);
+                  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
+                  if (imu_data_ptr != nullptr) {
+                    GetExtrapolator(trajectory_id)->AddImuData(*imu_data_ptr);
+                  }
                 })));
   }
 

cartographer_ros/cartographer_ros/node.h

@@ -85,6 +85,7 @@ class Node {
                          const cartographer_ros_msgs::SensorTopics& topics,
                          int trajectory_id);
   void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
+  ::cartographer::mapping::PoseExtrapolator* GetExtrapolator(int trajectory_id);
   void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event);
   void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
   void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -16,6 +16,7 @@
 
 #include "cartographer_ros/sensor_bridge.h"
 
+#include "cartographer/common/make_unique.h"
 #include "cartographer_ros/msg_conversion.h"
 #include "cartographer_ros/time_conversion.h"
 
@@ -58,7 +59,7 @@ void SensorBridge::HandleOdometryMessage(
   }
 }
 
-void SensorBridge::HandleImuMessage(const string& sensor_id,
+std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
     const sensor_msgs::Imu::ConstPtr& msg) {
   CHECK_NE(msg->linear_acceleration_covariance[0], -1)
       << "Your IMU data claims to not contain linear acceleration measurements "
@@ -74,15 +75,27 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
   const carto::common::Time time = FromRos(msg->header.stamp);
   const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
       time, CheckNoLeadingSlash(msg->header.frame_id));
-  if (sensor_to_tracking != nullptr) {
+  if (sensor_to_tracking == nullptr) {
+    return nullptr;
+  }
   CHECK(sensor_to_tracking->translation().norm() < 1e-5)
       << "The IMU frame must be colocated with the tracking frame. "
          "Transforming linear acceleration into the tracking frame will "
          "otherwise be imprecise.";
-    trajectory_builder_->AddImuData(
-        sensor_id, time,
+  return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>(
+      ::cartographer::sensor::ImuData{
+          time,
           sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
-        sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity));
+          sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
+}
+
+void SensorBridge::HandleImuMessage(const string& sensor_id,
+                                    const sensor_msgs::Imu::ConstPtr& msg) {
+  std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
+  if (imu_data != nullptr) {
+    trajectory_builder_->AddImuData(sensor_id, imu_data->time,
+                                    imu_data->linear_acceleration,
+                                    imu_data->angular_velocity);
   }
 }
 

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -17,7 +17,10 @@
 #ifndef CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
 #define CARTOGRAPHER_ROS_SENSOR_BRIDGE_H_
 
+#include <memory>
+
 #include "cartographer/mapping/trajectory_builder.h"
+#include "cartographer/sensor/imu_data.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer_ros/tf_bridge.h"
@@ -45,6 +48,8 @@ class SensorBridge {
 
   void HandleOdometryMessage(const string& sensor_id,
                              const nav_msgs::Odometry::ConstPtr& msg);
+  std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
+      const sensor_msgs::Imu::ConstPtr& msg);
   void HandleImuMessage(const string& sensor_id,
                         const sensor_msgs::Imu::ConstPtr& msg);
   void HandleLaserScanMessage(const string& sensor_id,