Adding NavSatFix to trajectory builder. (#666)

GPS message is converted first to ECEF, and then to a local frame. The first GPS message defines the local frame. 

PAIR=wohe

[RFC=0007](https://github.com/googlecartographer/rfcs/blob/master/text/0007-nav-sat-support.md)

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -48,37 +48,57 @@ SensorBridge::SensorBridge(
       tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
       trajectory_builder_(trajectory_builder) {}
 
-std::unique_ptr<::cartographer::sensor::OdometryData>
-SensorBridge::ToOdometryData(const nav_msgs::Odometry::ConstPtr& msg) {
+std::unique_ptr<carto::sensor::OdometryData> 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) {
     return nullptr;
   }
-  return ::cartographer::common::make_unique<
-      ::cartographer::sensor::OdometryData>(
-      ::cartographer::sensor::OdometryData{
+  return carto::common::make_unique<carto::sensor::OdometryData>(
+      carto::sensor::OdometryData{
           time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
 }
 
 void SensorBridge::HandleOdometryMessage(
     const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
-  std::unique_ptr<::cartographer::sensor::OdometryData> odometry_data =
+  std::unique_ptr<carto::sensor::OdometryData> odometry_data =
       ToOdometryData(msg);
   if (odometry_data != nullptr) {
     trajectory_builder_->AddSensorData(
-        sensor_id, cartographer::sensor::OdometryData{odometry_data->time,
-                                                      odometry_data->pose});
+        sensor_id,
+        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
   }
 }
 
 void SensorBridge::HandleNavSatFixMessage(
     const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
-  LOG(FATAL) << "TODO(spielawa / wohe): NavSatFix support not yet implemented.";
+  const carto::common::Time time = FromRos(msg->header.stamp);
+  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
+    trajectory_builder_->AddSensorData(
+        sensor_id, carto::sensor::FixedFramePoseData{
+                       time, carto::common::optional<Rigid3d>()});
+    return;
+  }
+
+  if (!ecef_to_local_frame_.has_value()) {
+    ecef_to_local_frame_ =
+        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
+    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
+              << msg->latitude << ", long = " << msg->longitude << ".";
+  }
+
+  trajectory_builder_->AddSensorData(
+      sensor_id,
+      carto::sensor::FixedFramePoseData{
+          time, carto::common::optional<Rigid3d>(Rigid3d::Translation(
+                    ecef_to_local_frame_.value() *
+                    LatLongAltToEcef(msg->latitude, msg->longitude,
+                                     msg->altitude)))});
 }
 
-std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
+std::unique_ptr<carto::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 "
@@ -101,8 +121,8 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
       << "The IMU frame must be colocated with the tracking frame. "
          "Transforming linear acceleration into the tracking frame will "
          "otherwise be imprecise.";
-  return ::cartographer::common::make_unique<::cartographer::sensor::ImuData>(
-      ::cartographer::sensor::ImuData{
+  return carto::common::make_unique<carto::sensor::ImuData>(
+      carto::sensor::ImuData{
           time,
           sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
           sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
@@ -110,19 +130,19 @@ std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
 
 void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                     const sensor_msgs::Imu::ConstPtr& msg) {
-  std::unique_ptr<::cartographer::sensor::ImuData> imu_data = ToImuData(msg);
+  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
   if (imu_data != nullptr) {
     trajectory_builder_->AddSensorData(
-        sensor_id, cartographer::sensor::ImuData{imu_data->time,
-                                                 imu_data->linear_acceleration,
-                                                 imu_data->angular_velocity});
+        sensor_id,
+        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
+                               imu_data->angular_velocity});
   }
 }
 
 void SensorBridge::HandleLaserScanMessage(
     const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
-  ::cartographer::sensor::PointCloudWithIntensities point_cloud;
-  ::cartographer::common::Time time;
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
   std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
   HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 }
@@ -130,8 +150,8 @@ void SensorBridge::HandleLaserScanMessage(
 void SensorBridge::HandleMultiEchoLaserScanMessage(
     const std::string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  ::cartographer::sensor::PointCloudWithIntensities point_cloud;
-  ::cartographer::common::Time time;
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
   std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
   HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 }
@@ -187,7 +207,7 @@ void SensorBridge::HandleRangefinder(
       tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
   if (sensor_to_tracking != nullptr) {
     trajectory_builder_->AddSensorData(
-        sensor_id, cartographer::sensor::TimedPointCloudData{
+        sensor_id, carto::sensor::TimedPointCloudData{
                        time, sensor_to_tracking->translation().cast<float>(),
                        carto::sensor::TransformTimedPointCloud(
                            ranges, sensor_to_tracking->cast<float>())});

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -19,6 +19,7 @@
 
 #include <memory>
 
+#include "cartographer/common/optional.h"
 #include "cartographer/mapping/trajectory_builder_interface.h"
 #include "cartographer/sensor/imu_data.h"
 #include "cartographer/sensor/odometry_data.h"
@@ -82,6 +83,9 @@ class SensorBridge {
   const TfBridge tf_bridge_;
   ::cartographer::mapping::TrajectoryBuilderInterface* const
       trajectory_builder_;
+
+  ::cartographer::common::optional<::cartographer::transform::Rigid3d>
+      ecef_to_local_frame_;
 };
 
 }  // namespace cartographer_ros