Add option to publish a pure 2D pose. (#683)

If the new `publish_frame_projected_to_2d` option is set to true,
the published pose will be restricted to a pure 2D pose
(no roll, pitch, or z-offset).
This prevents potentially unwanted out-of-plane poses in 2D mode
that can occur due to the pose extrapolation step (e.g. if the pose
shall be published as a 'base-footprint'-like frame).

cartographer_ros/cartographer_ros/node.cc

@@ -211,7 +211,13 @@ void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) {
     const ::cartographer::common::Time now = std::max(
         FromRos(ros::Time::now()), extrapolator.GetLastExtrapolatedTime());
     stamped_transform.header.stamp = ToRos(now);
-    const Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now);
+
+    Rigid3d tracking_to_local = extrapolator.ExtrapolatePose(now);
+    if (trajectory_state.trajectory_options.publish_frame_projected_to_2d) {
+      tracking_to_local = carto::transform::Embed3D(
+          carto::transform::Project2D(tracking_to_local));
+    }
+
     const Rigid3d tracking_to_map =
         trajectory_state.local_to_map * tracking_to_local;
 

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -54,6 +54,8 @@ 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.publish_frame_projected_to_2d =
+      lua_parameter_dictionary->GetBool("publish_frame_projected_to_2d");
   options.num_laser_scans =
       lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
   options.num_multi_echo_laser_scans =
@@ -108,6 +110,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->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;
   options->num_subdivisions_per_laser_scan =
@@ -136,6 +139,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.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;
   msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;

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 publish_frame_projected_to_2d;
   int num_laser_scans;
   int num_multi_echo_laser_scans;
   int num_subdivisions_per_laser_scan;

cartographer_ros/configuration_files/backpack_2d.lua

@@ -23,6 +23,7 @@ options = {
   published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
   num_laser_scans = 0,

cartographer_ros/configuration_files/backpack_3d.lua

@@ -23,6 +23,7 @@ options = {
   published_frame = "base_link",
   odom_frame = "odom",
   provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
   num_laser_scans = 0,

cartographer_ros/configuration_files/pr2.lua

@@ -23,6 +23,7 @@ options = {
   published_frame = "base_footprint",
   odom_frame = "odom",
   provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
   num_laser_scans = 1,

cartographer_ros/configuration_files/revo_lds.lua

@@ -23,6 +23,7 @@ options = {
   published_frame = "horizontal_laser_link",
   odom_frame = "odom",
   provide_odom_frame = true,
+  publish_frame_projected_to_2d = false,
   use_odometry = false,
   use_nav_sat = false,
   num_laser_scans = 1,

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -23,6 +23,7 @@ options = {
   published_frame = "odom",
   odom_frame = "odom",
   provide_odom_frame = false,
+  publish_frame_projected_to_2d = false,
   use_odometry = true,
   use_nav_sat = false,
   num_laser_scans = 1,

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 publish_frame_projected_to_2d
 int32 num_laser_scans
 int32 num_multi_echo_laser_scans
 int32 num_subdivisions_per_laser_scan