Class TwistSubscriber

Class Documentation

class TwistSubscriber

A simple wrapper on a Twist subscriber that receives either geometry_msgs::msg::TwistStamped or geometry_msgs::msg::Twist.

Note

Usage: The default behavior is to subscribe to Twist, which preserves backwards compatibility with ROS distributions up to Iron. The behavior can be overridden using the “enable_stamped_cmd_vel” parameter. By setting that to “True”, the TwistSubscriber class would subscribe to TwistStamped.

Note

Why use Twist Stamped over Twist? Twist has been used widely in many ROS applications, typically for body-frame velocity control, and between ROS nodes on the same computer. Many ROS interfaces are moving to using TwistStamped because it is more robust for stale data protection. This protection is especially important when sending velocity control over lossy communication links. An example application where this matters is a drone with a Linux computer running a ROS controller that sends Twist commands to an embedded autopilot. If the autopilot failed to recognize a highly latent connection, it could result in instability or a crash because of the decreased phase margin for control. TwistStamped also has a frame ID, allowing explicit control for multiple frames, rather than relying on an assumption of body-frame control or having to create a different topic. Adding a header is low-cost for most ROS applications; the header can be set to an empty string if bandwidth is of concern.

Note

Implementation Design Notes: Compared to the naive approach of setting up one subscriber for each message type, only one subscriber is created at a time; the other is nullptr. This reduces RAM usage and ROS discovery traffic. This approach allows NAV2 libraries to be flexible in which Twist message they support, while maintaining a stable API in a ROS distribution.

Public Functions

template<typename TwistCallbackT, typename TwistStampedCallbackT>
inline explicit TwistSubscriber(nav2_util::LifecycleNode::SharedPtr node, const std::string &topic, const rclcpp::QoS &qos, TwistCallbackT &&TwistCallback, TwistStampedCallbackT &&TwistStampedCallback)

A constructor that supports either Twist and TwistStamped.

Parameters:
  • node – The node to add the Twist subscriber to

  • topic – The subscriber topic name

  • qos – The subscriber quality of service

  • TwistCallback – The subscriber callback for Twist messages

  • TwistStampedCallback – The subscriber callback for TwistStamped messages

template<typename TwistStampedCallbackT>
inline explicit TwistSubscriber(nav2_util::LifecycleNode::SharedPtr node, const std::string &topic, const rclcpp::QoS &qos, TwistStampedCallbackT &&TwistStampedCallback)

A constructor that only supports TwistStamped.

Parameters:
  • node – The node to add the TwistStamped subscriber to

  • topic – The subscriber topic name

  • qos – The subscriber quality of service

  • TwistStampedCallback – The subscriber callback for TwistStamped messages

Throws:

std::invalid_argument – When configured with an invalid ROS parameter

Protected Attributes

bool is_stamped_ = {false}

The user-configured value for ROS parameter enable_stamped_cmd_vel.

rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_ = {nullptr}

The subscription when using Twist.

rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_stamped_sub_ = {nullptr}

The subscription when using TwistStamped.