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