Pass ROS landmark topic to the cartographer. (#746)

[Landmark RFC](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -40,7 +40,6 @@
 #include "sensor_msgs/PointCloud2.h"
 
 namespace cartographer_ros {
-
 namespace {
 
 // The ros::sensor_msgs::PointCloud2 binary data contains 4 floats for each
@@ -48,8 +47,12 @@ namespace {
 // properly.
 constexpr float kPointCloudComponentFourMagic = 1.;
 
+using ::cartographer::sensor::LandmarkData;
+using ::cartographer::sensor::LandmarkObservation;
 using ::cartographer::sensor::PointCloudWithIntensities;
 using ::cartographer::transform::Rigid3d;
+using ::cartographer_ros_msgs::LandmarkEntry;
+using ::cartographer_ros_msgs::LandmarkList;
 
 sensor_msgs::PointCloud2 PreparePointCloud2Message(const int64_t timestamp,
                                                    const std::string& frame_id,
@@ -205,6 +208,17 @@ ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) {
   return std::make_tuple(point_cloud, FromRos(message.header.stamp));
 }
 
+LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {
+  LandmarkData landmark_data;
+  landmark_data.time = FromRos(landmark_list.header.stamp);
+  for (const LandmarkEntry& entry : landmark_list.landmark) {
+    landmark_data.landmark_observations.push_back(
+        {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
+         entry.translation_weight, entry.rotation_weight});
+  }
+  return landmark_data;
+}
+
 Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {
   return Rigid3d(ToEigen(transform.transform.translation),
                  ToEigen(transform.transform.rotation));

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -20,8 +20,10 @@
 #include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/io/submap_painter.h"
+#include "cartographer/sensor/landmark_data.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
+#include "cartographer_ros_msgs/LandmarkList.h"
 #include "geometry_msgs/Pose.h"
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
@@ -63,6 +65,9 @@ std::tuple<::cartographer::sensor::PointCloudWithIntensities,
            ::cartographer::common::Time>
 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message);
 
+::cartographer::sensor::LandmarkData ToLandmarkData(
+    const cartographer_ros_msgs::LandmarkList& landmark_list);
+
 ::cartographer::transform::Rigid3d ToRigid3d(
     const geometry_msgs::TransformStamped& transform);
 

cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -15,16 +15,28 @@
  */
 
 #include "cartographer_ros/msg_conversion.h"
+#include "cartographer/transform/rigid_transform_test_helpers.h"
 
 #include <cmath>
 #include <random>
 
+#include "cartographer_ros/time_conversion.h"
+#include "gmock/gmock.h"
 #include "gtest/gtest.h"
 #include "sensor_msgs/LaserScan.h"
 
 namespace cartographer_ros {
 namespace {
 
+using ::cartographer::sensor::LandmarkData;
+using ::cartographer::sensor::LandmarkObservation;
+using ::testing::AllOf;
+using ::testing::DoubleNear;
+using ::testing::ElementsAre;
+using ::testing::Field;
+
+constexpr double kEps = 1e-6;
+
 TEST(MsgConversion, LaserScanToPointCloud) {
   sensor_msgs::LaserScan laser_scan;
   for (int i = 0; i < 8; ++i) {
@@ -39,25 +51,25 @@ TEST(MsgConversion, LaserScanToPointCloud) {
   const auto point_cloud =
       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   EXPECT_TRUE(
-      point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), 1e-6));
+      point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), kEps));
   EXPECT_TRUE(point_cloud[1].isApprox(
       Eigen::Vector4f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
-      1e-6));
+      kEps));
   EXPECT_TRUE(
-      point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), 1e-6));
+      point_cloud[2].isApprox(Eigen::Vector4f(0.f, 1.f, 0.f, 0.f), kEps));
   EXPECT_TRUE(point_cloud[3].isApprox(
       Eigen::Vector4f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f, 0.f),
-      1e-6));
+      kEps));
   EXPECT_TRUE(
-      point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), 1e-6));
+      point_cloud[4].isApprox(Eigen::Vector4f(-1.f, 0.f, 0.f, 0.f), kEps));
   EXPECT_TRUE(point_cloud[5].isApprox(
       Eigen::Vector4f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
-      1e-6));
+      kEps));
   EXPECT_TRUE(
-      point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), 1e-6));
+      point_cloud[6].isApprox(Eigen::Vector4f(0.f, -1.f, 0.f, 0.f), kEps));
   EXPECT_TRUE(point_cloud[7].isApprox(
       Eigen::Vector4f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f, 0.f),
-      1e-6));
+      kEps));
 }
 
 TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
@@ -77,9 +89,53 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
       std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   ASSERT_EQ(2, point_cloud.size());
   EXPECT_TRUE(
-      point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), 1e-6));
+      point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), kEps));
   EXPECT_TRUE(
-      point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), 1e-6));
+      point_cloud[1].isApprox(Eigen::Vector4f(-3.f, 0.f, 0.f, 0.f), kEps));
+}
+
+::testing::Matcher<const LandmarkObservation&> EqualsLandmark(
+    const LandmarkObservation& expected) {
+  return ::testing::AllOf(
+      Field(&LandmarkObservation::id, expected.id),
+      Field(&LandmarkObservation::landmark_to_tracking_transform,
+            ::cartographer::transform::IsNearly(
+                expected.landmark_to_tracking_transform, kEps)),
+      Field(&LandmarkObservation::translation_weight,
+            DoubleNear(expected.translation_weight, kEps)),
+      Field(&LandmarkObservation::rotation_weight,
+            DoubleNear(expected.rotation_weight, kEps)));
+}
+
+TEST(MsgConversion, LandmarkListToLandmarkData) {
+  cartographer_ros_msgs::LandmarkList message;
+  message.header.stamp.fromSec(10);
+
+  cartographer_ros_msgs::LandmarkEntry landmark_0;
+  landmark_0.id = "landmark_0";
+  landmark_0.tracking_from_landmark_transform.position.x = 1.0;
+  landmark_0.tracking_from_landmark_transform.position.y = 2.0;
+  landmark_0.tracking_from_landmark_transform.position.z = 3.0;
+  landmark_0.tracking_from_landmark_transform.orientation.w = 1.0;
+  landmark_0.tracking_from_landmark_transform.orientation.x = 0.0;
+  landmark_0.tracking_from_landmark_transform.orientation.y = 0.0;
+  landmark_0.tracking_from_landmark_transform.orientation.z = 0.0;
+  landmark_0.translation_weight = 1.0;
+  landmark_0.rotation_weight = 2.0;
+  message.landmark.push_back(landmark_0);
+
+  LandmarkData actual_landmark_data = ToLandmarkData(message);
+  EXPECT_THAT(actual_landmark_data,
+              AllOf(Field(&LandmarkData::time, FromRos(message.header.stamp)),
+                    Field(&LandmarkData::landmark_observations,
+                          ElementsAre(EqualsLandmark(LandmarkObservation{
+                              "landmark_0",
+                              ::cartographer::transform::Rigid3d(
+                                  Eigen::Vector3d(1., 2., 3.),
+                                  Eigen::Quaterniond(1., 0., 0., 0.)),
+                              1.f,
+                              2.f,
+                          })))));
 }
 
 TEST(MsgConversion, LatLongAltToEcef) {
@@ -90,23 +146,23 @@ TEST(MsgConversion, LatLongAltToEcef) {
   EXPECT_TRUE(plus_ten_meters.isApprox(Eigen::Vector3d(6378147, 0, 0)))
       << plus_ten_meters;
   Eigen::Vector3d north_pole = LatLongAltToEcef(90, 0, 0);
-  EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9))
+  EXPECT_TRUE(north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
       << north_pole;
   Eigen::Vector3d also_north_pole = LatLongAltToEcef(90, 90, 0);
   EXPECT_TRUE(
-      also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), 1e-9))
+      also_north_pole.isApprox(Eigen::Vector3d(0, 0, 6356752.3142), kEps))
       << also_north_pole;
   Eigen::Vector3d south_pole = LatLongAltToEcef(-90, 0, 0);
-  EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), 1e-9))
+  EXPECT_TRUE(south_pole.isApprox(Eigen::Vector3d(0, 0, -6356752.3142), kEps))
       << south_pole;
   Eigen::Vector3d above_south_pole = LatLongAltToEcef(-90, 60, 20);
   EXPECT_TRUE(
-      above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), 1e-9))
+      above_south_pole.isApprox(Eigen::Vector3d(0, 0, -6356772.3142), kEps))
       << above_south_pole;
   Eigen::Vector3d somewhere_on_earth =
       LatLongAltToEcef(48.1372149, 11.5748024, 517.1);
   EXPECT_TRUE(somewhere_on_earth.isApprox(
-      Eigen::Vector3d(4177983, 855702, 4727457), 1e-6))
+      Eigen::Vector3d(4177983, 855702, 4727457), kEps))
       << somewhere_on_earth;
 }
 
@@ -143,7 +199,7 @@ TEST(MsgConversion, ComputeLocalFrameFromLatLong) {
         ComputeLocalFrameFromLatLong(latitude, longitude);
     EXPECT_TRUE((transform_to_local_frame *
                  LatLongAltToEcef(latitude, longitude, altitude))
-                    .isApprox(altitude * Eigen::Vector3d::UnitZ(), 1e-6))
+                    .isApprox(altitude * Eigen::Vector3d::UnitZ(), kEps))
         << transform_to_local_frame *
                LatLongAltToEcef(latitude, longitude, altitude)
         << "\n"

cartographer_ros/cartographer_ros/node.cc

@@ -55,6 +55,7 @@ cartographer_ros_msgs::SensorTopics DefaultSensorTopics() {
   topics.imu_topic = kImuTopic;
   topics.odometry_topic = kOdometryTopic;
   topics.nav_sat_fix_topic = kNavSatFixTopic;
+  topics.landmark_topic = kLandmarkTopic;
   return topics;
 }
 
@@ -171,7 +172,8 @@ void Node::AddSensorSamplers(const int trajectory_id,
       std::piecewise_construct, std::forward_as_tuple(trajectory_id),
       std::forward_as_tuple(
           options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
-          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio));
+          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
+          options.landmarks_sampling_ratio));
 }
 
 void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
@@ -318,7 +320,10 @@ Node::ComputeExpectedSensorIds(
     expected_topics.insert(
         SensorId{SensorType::FIXED_FRAME_POSE, topics.nav_sat_fix_topic});
   }
-
+  // Landmark is optional.
+  if (options.use_landmarks) {
+    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
+  }
   return expected_topics;
 }
 
@@ -397,6 +402,14 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
              this),
          topic});
   }
+  if (options.use_landmarks) {
+    std::string topic = topics.landmark_topic;
+    subscribers_[trajectory_id].push_back(
+        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
+             &Node::HandleLandmarkMessage, trajectory_id, topic, &node_handle_,
+             this),
+         topic});
+  }
 }
 
 bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
@@ -603,6 +616,17 @@ void Node::HandleNavSatFixMessage(const int trajectory_id,
       ->HandleNavSatFixMessage(sensor_id, msg);
 }
 
+void Node::HandleLandmarkMessage(
+    const int trajectory_id, const std::string& sensor_id,
+    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
+  carto::common::MutexLocker lock(&mutex_);
+  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
+    return;
+  }
+  map_builder_bridge_.sensor_bridge(trajectory_id)
+      ->HandleLandmarkMessage(sensor_id, msg);
+}
+
 void Node::HandleImuMessage(const int trajectory_id,
                             const std::string& sensor_id,
                             const sensor_msgs::Imu::ConstPtr& msg) {

cartographer_ros/cartographer_ros/node.h

@@ -95,6 +95,9 @@ class Node {
                              const nav_msgs::Odometry::ConstPtr& msg);
   void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
                               const sensor_msgs::NavSatFix::ConstPtr& msg);
+  void HandleLandmarkMessage(
+      int trajectory_id, const std::string& sensor_id,
+      const cartographer_ros_msgs::LandmarkList::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,
@@ -180,16 +183,19 @@ class Node {
     TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
                              const double odometry_sampling_ratio,
                              const double fixed_frame_pose_sampling_ratio,
-                             const double imu_sampling_ratio)
+                             const double imu_sampling_ratio,
+                             const double landmark_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) {}
+          imu_sampler(imu_sampling_ratio),
+          landmark_sampler(landmark_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::common::FixedRatioSampler landmark_sampler;
   };
 
   // These are keyed with 'trajectory_id'.

cartographer_ros/cartographer_ros/node_constants.h

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

cartographer_ros/cartographer_ros/offline_node.cc

@@ -292,6 +292,11 @@ void RunOfflineNode(const MapBuilderFactory& map_builder_factory) {
         node.HandleNavSatFixMessage(trajectory_id, sensor_id,
                                     msg.instantiate<sensor_msgs::NavSatFix>());
       }
+      if (msg.isType<cartographer_ros_msgs::LandmarkList>()) {
+        node.HandleLandmarkMessage(
+            trajectory_id, sensor_id,
+            msg.instantiate<cartographer_ros_msgs::LandmarkList>());
+      }
     }
     clock.clock = msg.getTime();
     clock_publisher.publish(clock);

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -98,6 +98,12 @@ void SensorBridge::HandleNavSatFixMessage(
                                      msg->altitude)))});
 }
 
+void SensorBridge::HandleLandmarkMessage(
+    const std::string& sensor_id,
+    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
+  trajectory_builder_->AddSensorData(sensor_id, ToLandmarkData(*msg));
+}
+
 std::unique_ptr<carto::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

@@ -26,6 +26,7 @@
 #include "cartographer/transform/rigid_transform.h"
 #include "cartographer/transform/transform.h"
 #include "cartographer_ros/tf_bridge.h"
+#include "cartographer_ros_msgs/LandmarkList.h"
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
 #include "nav_msgs/OccupancyGrid.h"
@@ -55,6 +56,10 @@ class SensorBridge {
                              const nav_msgs::Odometry::ConstPtr& msg);
   void HandleNavSatFixMessage(const std::string& sensor_id,
                               const sensor_msgs::NavSatFix::ConstPtr& msg);
+  void HandleLandmarkMessage(
+      const std::string& sensor_id,
+      const cartographer_ros_msgs::LandmarkList::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

@@ -54,6 +54,7 @@ TrajectoryOptions CreateTrajectoryOptions(
       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.use_landmarks = lua_parameter_dictionary->GetBool("use_landmarks");
   options.publish_frame_projected_to_2d =
       lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
   options.num_laser_scans =
@@ -73,6 +74,8 @@ TrajectoryOptions CreateTrajectoryOptions(
       lua_parameter_dictionary->GetDouble("fixed_frame_pose_sampling_ratio");
   options.imu_sampling_ratio =
       lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
+  options.landmarks_sampling_ratio =
+      lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
   CheckTrajectoryOptions(options);
   return options;
 }
@@ -110,6 +113,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->provide_odom_frame = msg.provide_odom_frame;
   options->use_odometry = msg.use_odometry;
   options->use_nav_sat = msg.use_nav_sat;
+  options->use_landmarks = msg.use_landmarks;
   options->publish_frame_projected_to_2d = msg.publish_frame_projected_to_2d;
   options->num_laser_scans = msg.num_laser_scans;
   options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
@@ -121,6 +125,7 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->fixed_frame_pose_sampling_ratio =
       msg.fixed_frame_pose_sampling_ratio;
   options->imu_sampling_ratio = msg.imu_sampling_ratio;
+  options->landmarks_sampling_ratio = msg.landmarks_sampling_ratio;
   if (!options->trajectory_builder_options.ParseFromString(
           msg.trajectory_builder_options_proto)) {
     LOG(ERROR) << "Failed to parse protobuf";
@@ -139,6 +144,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
   msg.provide_odom_frame = options.provide_odom_frame;
   msg.use_odometry = options.use_odometry;
   msg.use_nav_sat = options.use_nav_sat;
+  msg.use_landmarks = options.use_landmarks;
   msg.publish_frame_projected_to_2d = options.publish_frame_projected_to_2d;
   msg.num_laser_scans = options.num_laser_scans;
   msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
@@ -148,6 +154,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
   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;
+  msg.landmarks_sampling_ratio = options.landmarks_sampling_ratio;
   options.trajectory_builder_options.SerializeToString(
       &msg.trajectory_builder_options_proto);
   return msg;

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -35,6 +35,7 @@ struct TrajectoryOptions {
   bool provide_odom_frame;
   bool use_odometry;
   bool use_nav_sat;
+  bool use_landmarks;
   bool publish_frame_projected_to_2d;
   int num_laser_scans;
   int num_multi_echo_laser_scans;
@@ -44,6 +45,7 @@ struct TrajectoryOptions {
   double odometry_sampling_ratio;
   double fixed_frame_pose_sampling_ratio;
   double imu_sampling_ratio;
+  double landmarks_sampling_ratio;
 };
 
 ::cartographer::mapping::proto::InitialTrajectoryPose

cartographer_ros/configuration_files/backpack_2d.lua

@@ -26,6 +26,7 @@ options = {
   publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
+  use_landmarks = false,
   num_laser_scans = 0,
   num_multi_echo_laser_scans = 1,
   num_subdivisions_per_laser_scan = 10,
@@ -38,6 +39,7 @@ options = {
   odometry_sampling_ratio = 1.,
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/backpack_3d.lua

@@ -26,6 +26,7 @@ options = {
   publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
+  use_landmarks = false,
   num_laser_scans = 0,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -38,6 +39,7 @@ options = {
   odometry_sampling_ratio = 1.,
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
 }
 
 TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 160

cartographer_ros/configuration_files/pr2.lua

@@ -26,6 +26,7 @@ options = {
   publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
+  use_landmarks = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -38,6 +39,7 @@ options = {
   odometry_sampling_ratio = 1.,
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/revo_lds.lua

@@ -26,6 +26,7 @@ options = {
   publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
+  use_landmarks = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -38,6 +39,7 @@ options = {
   odometry_sampling_ratio = 1.,
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -26,6 +26,7 @@ options = {
   publish_frame_projected_to_2d = false,
   use_odometry = true,
   use_nav_sat = false,
+  use_landmarks = false,
   num_laser_scans = 1,
   num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
@@ -38,6 +39,7 @@ options = {
   odometry_sampling_ratio = 1.,
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
+  landmarks_sampling_ratio = 1.,
 }
 
 TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 180

cartographer_ros_msgs/msg/SensorTopics.msg

@@ -18,3 +18,4 @@ string point_cloud2_topic
 string imu_topic
 string odometry_topic
 string nav_sat_fix_topic
+string landmark_topic

cartographer_ros_msgs/msg/TrajectoryOptions.msg

@@ -18,6 +18,7 @@ string odom_frame
 bool provide_odom_frame
 bool use_odometry
 bool use_nav_sat
+bool use_landmarks
 bool publish_frame_projected_to_2d
 int32 num_laser_scans
 int32 num_multi_echo_laser_scans
@@ -27,6 +28,7 @@ float64 rangefinder_sampling_ratio
 float64 odometry_sampling_ratio
 float64 fixed_frame_pose_sampling_ratio
 float64 imu_sampling_ratio
+float64 landmarks_sampling_ratio
 
 # This is a binary-encoded
 # 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.