Also extrapolate based on odometry if available. (#479)

cartographer_ros/cartographer_ros/node.cc

@@ -448,8 +448,12 @@ void Node::HandleOdometryMessage(const int trajectory_id,
                                  const string& sensor_id,
                                  const nav_msgs::Odometry::ConstPtr& msg) {
   carto::common::MutexLocker lock(&mutex_);
-  map_builder_bridge_.sensor_bridge(trajectory_id)
+  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
-      ->HandleOdometryMessage(sensor_id, msg);
+  auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
+  if (odometry_data_ptr != nullptr) {
+    extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
+  }
+  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
 }
 
 void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id,

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -47,15 +47,27 @@ SensorBridge::SensorBridge(
       tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
       trajectory_builder_(trajectory_builder) {}
 
-void SensorBridge::HandleOdometryMessage(
+std::unique_ptr<::cartographer::sensor::OdometryData>
-    const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
+SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {
   const carto::common::Time time = FromRos(msg->header.stamp);
   const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
       time, CheckNoLeadingSlash(msg->child_frame_id));
-  if (sensor_to_tracking != nullptr) {
+  if (sensor_to_tracking == nullptr) {
-    trajectory_builder_->AddOdometerData(
+    return nullptr;
-        sensor_id, time,
+  }
-        ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse());
+  return ::cartographer::common::make_unique<
+      ::cartographer::sensor::OdometryData>(
+      ::cartographer::sensor::OdometryData{
+          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
+}
+
+void SensorBridge::HandleOdometryMessage(
+    const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
+  std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
+      ToOdometryData(msg);
+  if (odometry_data != nullptr) {
+    trajectory_builder_->AddOdometerData(sensor_id, odometry_data->time,
+                                         odometry_data->pose);
   }
 }
 

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -21,6 +21,7 @@
 
 #include "cartographer/mapping/trajectory_builder.h"
 #include "cartographer/sensor/imu_data.h"
+#include "cartographer/sensor/odometry_data.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer_ros/tf_bridge.h"
@@ -46,6 +47,8 @@ class SensorBridge {
   SensorBridge(const SensorBridge&) = delete;
   SensorBridge& operator=(const SensorBridge&) = delete;
 
+  std::unique_ptr<::cartographer::sensor::OdometryData> ToOdometryData(
+      const nav_msgs::Odometry::ConstPtr& msg);
   void HandleOdometryMessage(const string& sensor_id,
                              const nav_msgs::Odometry::ConstPtr& msg);
   std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(