From dcbd9d10dfcd46dcbf883410a6b2ae397a8e2de7 Mon Sep 17 00:00:00 2001 From: Jonathan Huber Date: Wed, 21 Oct 2020 14:03:15 +0200 Subject: [PATCH] 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 | 61 ++++++++++++------- cartographer_ros/cartographer_ros/node.h | 1 + .../cartographer_ros/node_constants.h | 1 + .../cartographer_ros/node_options.cc | 8 +++ .../cartographer_ros/node_options.h | 2 + docs/source/configuration.rst | 7 +++ docs/source/ros_api.rst | 11 +++- 7 files changed, 65 insertions(+), 26 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 1bc804a..3354729 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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 stamped_transforms; + if (node_options_.publish_to_tf) { + if (trajectory_data.trajectory_options.provide_odom_frame) { + std::vector 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); } } } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index ac715d0..b02ca96 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/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_; diff --git a/cartographer_ros/cartographer_ros/node_constants.h b/cartographer_ros/cartographer_ros/node_constants.h index f54a4ab..07a85f3 100644 --- a/cartographer_ros/cartographer_ros/node_constants.h +++ b/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"; diff --git a/cartographer_ros/cartographer_ros/node_options.cc b/cartographer_ros/cartographer_ros/node_options.cc index 4812d08..17fae24 100644 --- a/cartographer_ros/cartographer_ros/node_options.cc +++ b/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"); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 43dc219..95bb159 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/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; }; diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 0d34b25..f3b27e6 100644 --- a/docs/source/configuration.rst +++ b/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 diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index 6e89f8e..7227d01 100644 --- a/docs/source/ros_api.rst +++ b/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 ` *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 ` *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