Class Pose2DPublisher

Inheritance Relationships

Base Type

  • public fuse_core::AsyncPublisher

Class Documentation

class Pose2DPublisher : public fuse_core::AsyncPublisher

Publisher plugin that publishes the latest 2D pose (combination of Position2DStamped and Orientation2DStamped)

There are several options: the latest pose can be sent to the tf topic, just the pose can be published, or the pose and the covariance can be published.

The tf operation is not straight forward. The standard ROS frame conventions are map->odom->base_link. The odom->base_link frame is generally published by a separate odometry system (robot driver node, robot_localization node, robot_pose_ekf node, etc.). If this node published a map->base_link transform, the tf system would no longer form a tree (the base_link node would have multiple parents), which breaks the tf system. Instead, we want to publish the map->odom transform. If the map->base_link transform is desired, the odom frame may be set to an empty string. Additionally, the tf system requires transforms to be available both before and after a requested timestamp before it will compute a transform. If this plugin only published transforms after each successful cycle of the optimizer, the map->odom transform will become old or stale as the optimization time increases. To prevent this, the map->odom transform is published in response to an independent timer, and the timestamp of the transform is updated to the current publication time. Although this is “wrong”, it keeps the tf tree populated with recent transform data so that other nodes can execute tf queries.

Parameters:

  • base_frame (string, default: base_link) Name for the robot’s base frame

  • device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish

  • device_name (string) Used to generate the device/robot ID if the device_id is not provided

  • map_frame (string, default: map) Name for the robot’s map frame

  • odom_frame (string, default: odom) Name for the robot’s odom frame (or {empty} if the frame map->base should be published to tf instead of map->odom)

  • publish_to_tf (bool, default: false) Flag indicating that the optimized pose should be published to tf

  • tf_cache_time (seconds, default: 10.0) How long to keep a history of transforms (for map->odom lookup)

  • tf_publish_frequency (Hz, default: 10.0) How often the latest pose should be published to tf

  • tf_timeout (seconds, default: 0.1) The maximum amount of time to wait for a transform to become available

Publishes:

  • pose (geometry_msgs::msg::PoseStamped) The most recent optimized robot pose (i.e. the map->base transform)

  • pose_with_covariance (geometry_msgs::msg::PoseWithCovarianceStamped) The most recent optimized robot pose and covariance (i.e. the map->base transform)

  • tf (tf2_msgs::msg::TFMessage) The most recent map->odom transform (or map->base if the odom_frame is empty)

Subscribes:

  • tf, tf_static (tf2_msgs::msg::TFMessage) Used to lookup the current odom->base frame, if needed

Public Functions

Pose2DPublisher()

Constructor.

virtual ~Pose2DPublisher() = default

Destructor.

void initialize(fuse_core::node_interfaces::NodeInterfaces<ALL_FUSE_CORE_NODE_INTERFACES> interfaces, const std::string &name) override

Shadowing extension to the AsyncPublisher::initialize call.

virtual void onInit() override

Perform any required post-construction initialization, such as advertising publishers or reading from the parameter server.

virtual void onStart() override

Perform any required operations before the first call to notify() occurs.

virtual void onStop() override

Perform any required operations to stop publications.

void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, fuse_core::Graph::ConstSharedPtr graph) override

Notify the publisher about variables that have been added or removed.

This publisher publishes only the most recent pose. By analyzing the added and removed variables, the most recent pose UUID can be maintained without performing an exhaustive search through the entire Graph during the Publisher::publish() call.

Parameters:

  • transaction – [in] A Transaction object, describing the set of variables that have been added and/or removed

  • graph – [in] A read-only pointer to the graph object, allowing queries to be performed whenever needed

void tfPublishTimerCallback()

Timer-based callback that publishes the latest map->odom transform.

The transform is published with the current timestamp, not the timestamp of the data used to generate the transform. Although this is “wrong”, it keeps the tf tree populated with recent transforms so that other nodes can execute tf queries.

Protected Types

using Synchronizer = StampedVariableSynchronizer<fuse_variables::Orientation2DStamped, fuse_variables::Position2DStamped>

Protected Attributes

fuse_core::node_interfaces::NodeInterfaces<fuse_core::node_interfaces::Base, fuse_core::node_interfaces::Clock, fuse_core::node_interfaces::Logging, fuse_core::node_interfaces::Parameters, fuse_core::node_interfaces::Timers, fuse_core::node_interfaces::Topics, fuse_core::node_interfaces::Waitables> interfaces_

Shadows AsyncPublisher interfaces_.

std::string base_frame_

The name of the robot’s base_link frame.

fuse_core::UUID device_id_

The UUID of the device to be published.

rclcpp::Clock::SharedPtr clock_

The publisher’s clock, for timestamping and logging.

rclcpp::Logger logger_

The publisher’s logger.

std::string map_frame_

The name of the robot’s map frame.

std::string odom_frame_

The name of the odom frame for this pose (or empty if the odom is not used)

rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_with_covariance_publisher_
bool publish_to_tf_

Flag indicating the pose should be sent to the tf system as well as the pose topics

Synchronizer::UniquePtr synchronizer_

Object that tracks the latest common timestamp of multiple variables

std::unique_ptr<tf2_ros::Buffer> tf_buffer_

TF2 object that supports querying transforms by time and frame id

std::unique_ptr<tf2_ros::TransformListener> tf_listener_

Publish the map->odom or map->base transform to the tf system.

TF2 object that subscribes to the tf topics and inserts the received transforms into the tf buffer

std::shared_ptr<tf2_ros::TransformBroadcaster> tf_publisher_ = nullptr
rclcpp::TimerBase::SharedPtr tf_publish_timer_

Timer that publishes tf messages to ensure the tf transform doesn’t get stale

rclcpp::Duration tf_timeout_

The max time to wait for a tf transform to become available.

geometry_msgs::msg::TransformStamped tf_transform_

The transform to be published to tf.

bool use_tf_lookup_

Internal flag indicating that a tf frame lookup is required.