Wiring for sensor_msgs::NavSatFix (#659)

PAIR=wohe

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

cartographer_ros/cartographer_ros/node.cc

@@ -52,6 +52,7 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
   topics.point_cloud2_topic = kPointCloud2Topic;
   topics.imu_topic = kImuTopic;
   topics.odometry_topic = kOdometryTopic;
+  topics.nav_sat_fix_topic = kNavSatFixTopic;
   return topics;
 }
 
@@ -159,9 +160,9 @@ void Node::AddSensorSamplers(const int trajectory_id,
   CHECK(sensor_samplers_.count(trajectory_id) == 0);
   sensor_samplers_.emplace(
       std::piecewise_construct, std::forward_as_tuple(trajectory_id),
-      std::forward_as_tuple(options.rangefinder_sampling_ratio,
-                            options.odometry_sampling_ratio,
-                            options.imu_sampling_ratio));
+      std::forward_as_tuple(
+          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
+          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio));
 }
 
 void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
@@ -284,6 +285,11 @@ std::unordered_set<std::string> Node::ComputeExpectedTopics(
   if (options.use_odometry) {
     expected_topics.insert(topics.odometry_topic);
   }
+  // NavSatFix is optional.
+  if (options.use_nav_sat) {
+    expected_topics.insert(topics.nav_sat_fix_topic);
+  }
+
   return expected_topics;
 }
 
@@ -353,6 +359,14 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                                                   &node_handle_, this),
          topic});
   }
+  if (options.use_nav_sat) {
+    std::string topic = topics.nav_sat_fix_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
+             &Node::HandleNavSatFixMessage, trajectory_id, topic, &node_handle_,
+             this),
+         topic});
+  }
 }
 
 bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
@@ -502,6 +516,17 @@ void Node::HandleOdometryMessage(const int trajectory_id,
   sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
 }
 
+void Node::HandleNavSatFixMessage(const int trajectory_id,
+                                  const std::string& sensor_id,
+                                  const sensor_msgs::NavSatFix::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleNavSatFixMessage(sensor_id, msg);
+}
+
 void Node::HandleImuMessage(const int trajectory_id,
                             const std::string& sensor_id,
                             const sensor_msgs::Imu::ConstPtr& msg) {
@@ -529,7 +554,7 @@ void Node::HandleLaserScanMessage(const int trajectory_id,
 }
 
 void Node::HandleMultiEchoLaserScanMessage(
-    int trajectory_id, const std::string& sensor_id,
+    const int trajectory_id, const std::string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
   carto::common::MutexLocker lock(&mutex_);
   if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {

cartographer_ros/cartographer_ros/node.h

@@ -39,7 +39,13 @@
 #include "cartographer_ros_msgs/SubmapQuery.h"
 #include "cartographer_ros_msgs/TrajectoryOptions.h"
 #include "cartographer_ros_msgs/WriteState.h"
+#include "nav_msgs/Odometry.h"
 #include "ros/ros.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/LaserScan.h"
+#include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/NavSatFix.h"
+#include "sensor_msgs/PointCloud2.h"
 #include "tf2_ros/transform_broadcaster.h"
 
 namespace cartographer_ros {
@@ -79,6 +85,8 @@ class Node {
   // The following functions handle adding sensor data to a trajectory.
   void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                              const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
+                              const sensor_msgs::NavSatFix::ConstPtr& msg);
   void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                         const sensor_msgs::Imu::ConstPtr& msg);
   void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
@@ -156,15 +164,18 @@ class Node {
   ::ros::Publisher scan_matched_point_cloud_publisher_;
 
   struct TrajectorySensorSamplers {
-    TrajectorySensorSamplers(double rangefinder_sampling_ratio,
-                             double odometry_sampling_ratio,
-                             double imu_sampling_ratio)
+    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
+                             const double odometry_sampling_ratio,
+                             const double fixed_frame_pose_sampling_ratio,
+                             const double imu_sampling_ratio)
         : rangefinder_sampler(rangefinder_sampling_ratio),
           odometry_sampler(odometry_sampling_ratio),
+          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
           imu_sampler(imu_sampling_ratio) {}
 
     ::cartographer::common::FixedRatioSampler rangefinder_sampler;
     ::cartographer::common::FixedRatioSampler odometry_sampler;
+    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
     ::cartographer::common::FixedRatioSampler imu_sampler;
   };
 

cartographer_ros/cartographer_ros/node_constants.h

@@ -28,6 +28,7 @@ constexpr char kMultiEchoLaserScanTopic[] = "echoes";
 constexpr char kPointCloud2Topic[] = "points2";
 constexpr char kImuTopic[] = "imu";
 constexpr char kOdometryTopic[] = "odom";
+constexpr char kNavSatFixTopic[] = "fix";
 constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
 constexpr char kOccupancyGridTopic[] = "map";
 constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";

cartographer_ros/cartographer_ros/offline_node.cc

@@ -177,6 +177,11 @@ void RunOfflineNode(
               trajectory_id, topic,
               delayed_msg.instantiate<nav_msgs::Odometry>());
         }
+        if (delayed_msg.isType<sensor_msgs::NavSatFix>()) {
+          node.HandleNavSatFixMessage(
+              trajectory_id, topic,
+              delayed_msg.instantiate<sensor_msgs::NavSatFix>());
+        }
         clock.clock = delayed_msg.getTime();
         clock_publisher.publish(clock);
 

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -73,6 +73,11 @@ void SensorBridge::HandleOdometryMessage(
   }
 }
 
+void SensorBridge::HandleNavSatFixMessage(
+    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
+  LOG(FATAL) << "TODO(spielawa / wohe): NavSatFix support not yet implemented.";
+}
+
 std::unique_ptr<::cartographer::sensor::ImuData> SensorBridge::ToImuData(
     const sensor_msgs::Imu::ConstPtr& msg) {
   CHECK_NE(msg->linear_acceleration_covariance[0], -1)

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -32,6 +32,7 @@
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/LaserScan.h"
 #include "sensor_msgs/MultiEchoLaserScan.h"
+#include "sensor_msgs/NavSatFix.h"
 #include "sensor_msgs/PointCloud2.h"
 
 namespace cartographer_ros {
@@ -51,6 +52,8 @@ class SensorBridge {
       const nav_msgs::Odometry::ConstPtr& msg);
   void HandleOdometryMessage(const std::string& sensor_id,
                              const nav_msgs::Odometry::ConstPtr& msg);
+  void HandleNavSatFixMessage(const std::string& sensor_id,
+                              const sensor_msgs::NavSatFix::ConstPtr& msg);
   std::unique_ptr<::cartographer::sensor::ImuData> ToImuData(
       const sensor_msgs::Imu::ConstPtr& msg);
   void HandleImuMessage(const std::string& sensor_id,

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -53,6 +53,7 @@ TrajectoryOptions CreateTrajectoryOptions(
   options.provide_odom_frame =
       lua_parameter_dictionary->GetBool("provide_odom_frame");
   options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
+  options.use_nav_sat = lua_parameter_dictionary->GetBool("use_nav_sat");
   options.num_laser_scans =
       lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
   options.num_multi_echo_laser_scans =
@@ -66,6 +67,8 @@ TrajectoryOptions CreateTrajectoryOptions(
       lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio");
   options.odometry_sampling_ratio =
       lua_parameter_dictionary->GetDouble("odometry_sampling_ratio");
+  options.fixed_frame_pose_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
   options.imu_sampling_ratio =
       lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
   CheckTrajectoryOptions(options);
@@ -104,6 +107,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->odom_frame = msg.odom_frame;
   options->provide_odom_frame = msg.provide_odom_frame;
   options->use_odometry = msg.use_odometry;
+  options->use_nav_sat = msg.use_nav_sat;
   options->num_laser_scans = msg.num_laser_scans;
   options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
   options->num_subdivisions_per_laser_scan =
@@ -111,6 +115,8 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->num_point_clouds = msg.num_point_clouds;
   options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio;
   options->odometry_sampling_ratio = msg.odometry_sampling_ratio;
+  options->fixed_frame_pose_sampling_ratio =
+      msg.fixed_frame_pose_sampling_ratio;
   options->imu_sampling_ratio = msg.imu_sampling_ratio;
   if (!options->trajectory_builder_options.ParseFromString(
           msg.trajectory_builder_options_proto)) {
@@ -129,12 +135,14 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
   msg.odom_frame = options.odom_frame;
   msg.provide_odom_frame = options.provide_odom_frame;
   msg.use_odometry = options.use_odometry;
+  msg.use_nav_sat = options.use_nav_sat;
   msg.num_laser_scans = options.num_laser_scans;
   msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
   msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
   msg.num_point_clouds = options.num_point_clouds;
   msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio;
   msg.odometry_sampling_ratio = options.odometry_sampling_ratio;
+  msg.fixed_frame_pose_sampling_ratio = options.fixed_frame_pose_sampling_ratio;
   msg.imu_sampling_ratio = options.imu_sampling_ratio;
   options.trajectory_builder_options.SerializeToString(
       &msg.trajectory_builder_options_proto);

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -34,12 +34,14 @@ struct TrajectoryOptions {
   std::string odom_frame;
   bool provide_odom_frame;
   bool use_odometry;
+  bool use_nav_sat;
   int num_laser_scans;
   int num_multi_echo_laser_scans;
   int num_subdivisions_per_laser_scan;
   int num_point_clouds;
   double rangefinder_sampling_ratio;
   double odometry_sampling_ratio;
+  double fixed_frame_pose_sampling_ratio;
   double imu_sampling_ratio;
 };
 

cartographer_ros/configuration_files/backpack_2d.lua

@@ -24,6 +24,7 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
+  use_nav_sat = false,
   num_laser_scans = 0,
   num_multi_echo_laser_scans = 1,
   num_subdivisions_per_laser_scan = 10,
@@ -34,6 +35,7 @@ options = {
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
 }
 

cartographer_ros/configuration_files/backpack_3d.lua

@@ -24,6 +24,7 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
+  use_nav_sat = false,
   num_laser_scans = 0,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -34,6 +35,7 @@ options = {
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
 }
 

cartographer_ros/configuration_files/pr2.lua

@@ -24,6 +24,7 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
+  use_nav_sat = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -34,6 +35,7 @@ options = {
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
 }
 

cartographer_ros/configuration_files/revo_lds.lua

@@ -24,6 +24,7 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
+  use_nav_sat = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -34,6 +35,7 @@ options = {
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
 }
 

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -24,6 +24,7 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = false,
   use_odometry = true,
+  use_nav_sat = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -34,6 +35,7 @@ options = {
   trajectory_publish_period_sec = 30e-3,
   rangefinder_sampling_ratio = 1.,
   odometry_sampling_ratio = 1.,
+  fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
 }
 

cartographer_ros_msgs/msg/SensorTopics.msg

@@ -17,3 +17,4 @@ string multi_echo_laser_scan_topic
 string point_cloud2_topic
 string imu_topic
 string odometry_topic
+string nav_sat_fix_topic

cartographer_ros_msgs/msg/TrajectoryOptions.msg

@@ -17,12 +17,14 @@ string published_frame
 string odom_frame
 bool provide_odom_frame
 bool use_odometry
+bool use_nav_sat
 int32 num_laser_scans
 int32 num_multi_echo_laser_scans
 int32 num_subdivisions_per_laser_scan
 int32 num_point_clouds
 float64 rangefinder_sampling_ratio
 float64 odometry_sampling_ratio
+float64 fixed_frame_pose_sampling_ratio
 float64 imu_sampling_ratio
 
 # This is a binary-encoded