Make publishing tf optional, enable publishing PoseStamped (#1099)

If the output of cartographer should be used as an input to an additional sensor fusion method,
using the published TF transform is inconvenient or in our specific use case even harmful
because we don't want to add the raw cartographer output to our TF tree.

With this change it becomes optional to broadcast to /tf.
Morever there is an option to publish the tracked frame pose as a PoseStamped message.
We add two new optional parameters:
- `publish_to_tf` if false no tf transform is broadcasted
-  `publish_tracked_pose` If set `true` a PoseStamped representing the position of the
tracked pose w.r.t. map frame is published.

If default launchers and settings are used this PR causes no change in the behavior.

cartographer_ros/cartographer_ros/node.cc

@@ -40,6 +40,7 @@
 #include "cartographer_ros/time_conversion.h"
 #include "cartographer_ros_msgs/StatusCode.h"
 #include "cartographer_ros_msgs/StatusResponse.h"
+#include "geometry_msgs/PoseStamped.h"
 #include "glog/logging.h"
 #include "nav_msgs/Odometry.h"
 #include "ros/serialization.h"
@@ -113,6 +114,11 @@ Node::Node(
   constraint_list_publisher_ =
       node_handle_.advertise<::visualization_msgs::MarkerArray>(
           kConstraintListTopic, kLatestOnlyPublisherQueueSize);
+  if (node_options_.publish_tracked_pose) {
+    tracked_pose_publisher_ =
+        node_handle_.advertise<::geometry_msgs::PoseStamped>(
+            kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
+  }
   service_servers_.push_back(node_handle_.advertiseService(
       kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
   service_servers_.push_back(node_handle_.advertiseService(
@@ -271,32 +277,41 @@ void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
         trajectory_data.local_to_map * tracking_to_local;
 
     if (trajectory_data.published_to_tracking != nullptr) {
-      if (trajectory_data.trajectory_options.provide_odom_frame) {
-        std::vector<geometry_msgs::TransformStamped> stamped_transforms;
+      if (node_options_.publish_to_tf) {
+        if (trajectory_data.trajectory_options.provide_odom_frame) {
+          std::vector<geometry_msgs::TransformStamped> stamped_transforms;
 
-        stamped_transform.header.frame_id = node_options_.map_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.odom_frame;
-        stamped_transform.transform =
-            ToGeometryMsgTransform(trajectory_data.local_to_map);
-        stamped_transforms.push_back(stamped_transform);
+          stamped_transform.header.frame_id = node_options_.map_frame;
+          stamped_transform.child_frame_id =
+              trajectory_data.trajectory_options.odom_frame;
+          stamped_transform.transform =
+              ToGeometryMsgTransform(trajectory_data.local_to_map);
+          stamped_transforms.push_back(stamped_transform);
 
-        stamped_transform.header.frame_id =
-            trajectory_data.trajectory_options.odom_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.published_frame;
-        stamped_transform.transform = ToGeometryMsgTransform(
-            tracking_to_local * (*trajectory_data.published_to_tracking));
-        stamped_transforms.push_back(stamped_transform);
+          stamped_transform.header.frame_id =
+              trajectory_data.trajectory_options.odom_frame;
+          stamped_transform.child_frame_id =
+              trajectory_data.trajectory_options.published_frame;
+          stamped_transform.transform = ToGeometryMsgTransform(
+              tracking_to_local * (*trajectory_data.published_to_tracking));
+          stamped_transforms.push_back(stamped_transform);
 
-        tf_broadcaster_.sendTransform(stamped_transforms);
-      } else {
-        stamped_transform.header.frame_id = node_options_.map_frame;
-        stamped_transform.child_frame_id =
-            trajectory_data.trajectory_options.published_frame;
-        stamped_transform.transform = ToGeometryMsgTransform(
-            tracking_to_map * (*trajectory_data.published_to_tracking));
-        tf_broadcaster_.sendTransform(stamped_transform);
+          tf_broadcaster_.sendTransform(stamped_transforms);
+        } else {
+          stamped_transform.header.frame_id = node_options_.map_frame;
+          stamped_transform.child_frame_id =
+              trajectory_data.trajectory_options.published_frame;
+          stamped_transform.transform = ToGeometryMsgTransform(
+              tracking_to_map * (*trajectory_data.published_to_tracking));
+          tf_broadcaster_.sendTransform(stamped_transform);
+        }
+      }
+      if (node_options_.publish_tracked_pose) {
+        ::geometry_msgs::PoseStamped pose_msg;
+        pose_msg.header.frame_id = node_options_.map_frame;
+        pose_msg.header.stamp = stamped_transform.header.stamp;
+        pose_msg.pose = ToGeometryMsgPose(tracking_to_map);
+        tracked_pose_publisher_.publish(pose_msg);
       }
     }
   }

cartographer_ros/cartographer_ros/node.h

@@ -188,6 +188,7 @@ class Node {
   ::ros::Publisher trajectory_node_list_publisher_;
   ::ros::Publisher landmark_poses_list_publisher_;
   ::ros::Publisher constraint_list_publisher_;
+  ::ros::Publisher tracked_pose_publisher_;
   // These ros::ServiceServers need to live for the lifetime of the node.
   std::vector<::ros::ServiceServer> service_servers_;
   ::ros::Publisher scan_matched_point_cloud_publisher_;

cartographer_ros/cartographer_ros/node_constants.h

@@ -34,6 +34,7 @@ constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";
 constexpr char kOccupancyGridTopic[] = "map";
 constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";
 constexpr char kSubmapListTopic[] = "submap_list";
+constexpr char kTrackedPoseTopic[] = "tracked_pose";
 constexpr char kSubmapQueryServiceName[] = "submap_query";
 constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";
 constexpr char kStartTrajectoryServiceName[] = "start_trajectory";

cartographer_ros/cartographer_ros/node_options.cc

@@ -40,6 +40,14 @@ NodeOptions CreateNodeOptions(
       lua_parameter_dictionary->GetDouble("pose_publish_period_sec");
   options.trajectory_publish_period_sec =
       lua_parameter_dictionary->GetDouble("trajectory_publish_period_sec");
+  if (lua_parameter_dictionary->HasKey("publish_to_tf")) {
+    options.publish_to_tf =
+        lua_parameter_dictionary->GetBool("publish_to_tf");
+  }
+  if (lua_parameter_dictionary->HasKey("publish_tracked_pose")) {
+    options.publish_tracked_pose =
+        lua_parameter_dictionary->GetBool("publish_tracked_pose");
+  }
   if (lua_parameter_dictionary->HasKey("use_pose_extrapolator")) {
     options.use_pose_extrapolator =
         lua_parameter_dictionary->GetBool("use_pose_extrapolator");

cartographer_ros/cartographer_ros/node_options.h

@@ -35,6 +35,8 @@ struct NodeOptions {
   double submap_publish_period_sec;
   double pose_publish_period_sec;
   double trajectory_publish_period_sec;
+  bool publish_to_tf = true;
+  bool publish_tracked_pose = false;
   bool use_pose_extrapolator = true;
 };
 

docs/source/configuration.rst

@@ -103,6 +103,12 @@ pose_publish_period_sec
   Interval in seconds at which to publish poses, e.g. 5e-3 for a frequency of
   200 Hz.
 
+publish_to_tf
+  Enable or disable providing of TF transforms.
+
+publish_tracked_pose_msg
+  Enable publishing of tracked pose as a `geometry_msgs/PoseStamped`_ to topic "tracked_pose".
+
 trajectory_publish_period_sec
   Interval in seconds at which to publish the trajectory markers, e.g. 30e-3
   for 30 milliseconds.
@@ -124,6 +130,7 @@ landmarks_sampling_ratio
 
 .. _REP 105: http://www.ros.org/reps/rep-0105.html
 .. _ROS Names: http://wiki.ros.org/Names
+.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html
 .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html
 .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
 .. _sensor_msgs/LaserScan: http://docs.ros.org/api/sensor_msgs/html/msg/LaserScan.html

docs/source/ros_api.rst

@@ -82,6 +82,10 @@ submap_list (`cartographer_ros_msgs/SubmapList`_)
   List of all submaps, including the pose and latest version number of each
   submap, across all trajectories.
 
+tracked_pose (`geometry_msgs/PoseStamped`_)
+  Only published if the parameter ``publish_tracked_pose`` is set to ``true``.
+  The pose of the tracked frame with respect to the map frame.
+
 Services
 --------
 
@@ -94,7 +98,7 @@ submap_query (`cartographer_ros_msgs/SubmapQuery`_)
   Fetches the requested submap.
 
 start_trajectory (`cartographer_ros_msgs/StartTrajectory`_)
-  Starts a trajectory using default sensor topics and the provided configuration. 
+  Starts a trajectory using default sensor topics and the provided configuration.
   An initial pose can be optionally specified. Returns an assigned trajectory ID.
 
 trajectory_query (`cartographer_ros_msgs/TrajectoryQuery`_)
@@ -131,9 +135,9 @@ Provided tf Transforms
 ----------------------
 
 The transformation between the :doc:`configured <configuration>` *map_frame*
-and *published_frame* is always provided.
+and *published_frame* is provided unless the parameter ``publish_to_tf`` is set to ``false``.
 
-If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous
+If *provide_odom_frame* is enabled in the :doc:`configuration`, additionally a continuous
 (i.e. unaffected by loop closure) transform between the :doc:`configured
 <configuration>` *odom_frame* and *published_frame* will be provided.
 
@@ -147,6 +151,7 @@ If *provide_odom_frame* is enabled in the :doc:`configuration`, a continuous
 .. _cartographer_ros_msgs/WriteState: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/WriteState.srv
 .. _cartographer_ros_msgs/GetTrajectoryStates: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/GetTrajectoryStates.srv
 .. _cartographer_ros_msgs/ReadMetrics: https://github.com/cartographer-project/cartographer_ros/blob/master/cartographer_ros_msgs/srv/ReadMetrics.srv
+.. _geometry_msgs/PoseStamped: http://docs.ros.org/api/geometry_msgs/html/msg/PoseStamped.html
 .. _nav_msgs/OccupancyGrid: http://docs.ros.org/api/nav_msgs/html/msg/OccupancyGrid.html
 .. _nav_msgs/Odometry: http://docs.ros.org/api/nav_msgs/html/msg/Odometry.html
 .. _sensor_msgs/Imu: http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html