Add TwistStamped support via TwistPublisher and TwistSubscriber

nav2_behaviors/README.md

@@ -13,3 +13,6 @@ The value of the centralized behavior server is to **share resources** amongst s
 See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
 
 See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
+
+The `TimedBehavior` template makes use of a [nav2_util::TwistPublisher](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).
+The `AssistedTeleop` behavior makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).

nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp

@@ -23,6 +23,7 @@
 #include "std_msgs/msg/empty.hpp"
 #include "nav2_behaviors/timed_behavior.hpp"
 #include "nav2_msgs/action/assisted_teleop.hpp"
+#include "nav2_util/twist_subscriber.hpp"
 
 namespace nav2_behaviors
 {
@@ -82,12 +83,6 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
     const geometry_msgs::msg::Twist & twist,
     double projection_time);
 
-  /**
-   * @brief Callback function for velocity subscriber
-   * @param msg received Twist message
-   */
-  void teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
-
   /**
    * @brief Callback function to preempt assisted teleop
    * @param msg empty message
@@ -100,11 +95,11 @@ class AssistedTeleop : public TimedBehavior<AssistedTeleopAction>
   double projection_time_;
   double simulation_time_step_;
 
-  geometry_msgs::msg::Twist teleop_twist_;
+  geometry_msgs::msg::TwistStamped teleop_twist_;
   bool preempt_teleop_{false};
 
   // subscribers
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+  std::shared_ptr<nav2_util::TwistSubscriber> vel_sub_;
   rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr preempt_teleop_sub_;
 
   rclcpp::Duration command_time_allowance_{0, 0};

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -24,6 +24,7 @@
 #include "nav2_msgs/action/drive_on_heading.hpp"
 #include "nav2_msgs/action/back_up.hpp"
 #include "nav2_util/node_utils.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 namespace nav2_behaviors
 {
@@ -125,23 +126,23 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       return ResultStatus{Status::SUCCEEDED, ActionT::Result::NONE};
     }
 
-    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-    cmd_vel->linear.y = 0.0;
-    cmd_vel->angular.z = 0.0;
-    cmd_vel->linear.x = command_speed_;
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel->twist.linear.y = 0.0;
+    cmd_vel->twist.angular.z = 0.0;
+    cmd_vel->twist.linear.x = command_speed_;
 
     geometry_msgs::msg::Pose2D pose2d;
     pose2d.x = current_pose.pose.position.x;
     pose2d.y = current_pose.pose.position.y;
     pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
-    if (!isCollisionFree(distance, cmd_vel.get(), pose2d)) {
+    if (!isCollisionFree(distance, cmd_vel->twist, pose2d)) {
       this->stopRobot();
       RCLCPP_WARN(this->logger_, "Collision Ahead - Exiting DriveOnHeading");
       return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
     }
 
-    this->vel_pub_->publish(std::move(cmd_vel));
+    this->vel_pub_->publish(*cmd_vel);
 
     return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
   }
@@ -162,7 +163,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    */
   bool isCollisionFree(
     const double & distance,
-    geometry_msgs::msg::Twist * cmd_vel,
+    const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d)
   {
     // Simulate ahead by simulate_ahead_time_ in this->cycle_frequency_ increments
@@ -174,7 +175,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     bool fetch_data = true;
 
     while (cycle_count < max_cycle_count) {
-      sim_position_change = cmd_vel->linear.x * (cycle_count / this->cycle_frequency_);
+      sim_position_change = cmd_vel.linear.x * (cycle_count / this->cycle_frequency_);
       pose2d.x = init_pose.x + sim_position_change * cos(init_pose.theta);
       pose2d.y = init_pose.y + sim_position_change * sin(init_pose.theta);
       cycle_count++;

nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp

@@ -22,6 +22,7 @@
 #include "nav2_behaviors/timed_behavior.hpp"
 #include "nav2_msgs/action/spin.hpp"
 #include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 namespace nav2_behaviors
 {
@@ -79,7 +80,7 @@ class Spin : public TimedBehavior<SpinAction>
    */
   bool isCollisionFree(
     const double & distance,
-    geometry_msgs::msg::Twist * cmd_vel,
+    const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d);
 
   SpinAction::Feedback::SharedPtr feedback_;

nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp

@@ -29,8 +29,9 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "geometry_msgs/msg/twist.hpp"
-#include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/simple_action_server.hpp"
 #include "nav2_core/behavior.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
@@ -150,7 +151,7 @@ class TimedBehavior : public nav2_core::Behavior
     local_collision_checker_ = local_collision_checker;
     global_collision_checker_ = global_collision_checker;
 
-    vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+    vel_pub_ = std::make_unique<nav2_util::TwistPublisher>(node, "cmd_vel", 1);
 
     onConfigure();
   }
@@ -185,7 +186,7 @@ class TimedBehavior : public nav2_core::Behavior
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
 
   std::string behavior_name_;
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+  std::unique_ptr<nav2_util::TwistPublisher> vel_pub_;
   std::shared_ptr<ActionServer> action_server_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> local_collision_checker_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> global_collision_checker_;
@@ -289,12 +290,14 @@ class TimedBehavior : public nav2_core::Behavior
   // Stop the robot with a commanded velocity
   void stopRobot()
   {
-    auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-    cmd_vel->linear.x = 0.0;
-    cmd_vel->linear.y = 0.0;
-    cmd_vel->angular.z = 0.0;
-
-    vel_pub_->publish(std::move(cmd_vel));
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel->header.frame_id = "base_link";
+    cmd_vel->header.stamp = rclcpp::Time();
+    cmd_vel->twist.linear.x = 0.0;
+    cmd_vel->twist.linear.y = 0.0;
+    cmd_vel->twist.angular.z = 0.0;
+
+    vel_pub_->publish(*cmd_vel);
   }
 };
 

nav2_behaviors/plugins/assisted_teleop.cpp

@@ -50,11 +50,15 @@ void AssistedTeleop::onConfigure()
   std::string cmd_vel_teleop;
   node->get_parameter("cmd_vel_teleop", cmd_vel_teleop);
 
-  vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(
+
+  vel_sub_ = std::make_shared<nav2_util::TwistSubscriber>(
+    node,
     cmd_vel_teleop, rclcpp::SystemDefaultsQoS(),
-    std::bind(
-      &AssistedTeleop::teleopVelocityCallback,
-      this, std::placeholders::_1));
+    [&](geometry_msgs::msg::Twist::SharedPtr msg) {
+      teleop_twist_.twist = *msg;
+    }, [&](geometry_msgs::msg::TwistStamped::SharedPtr msg) {
+      teleop_twist_ = *msg;
+    });
 
   preempt_teleop_sub_ = node->create_subscription<std_msgs::msg::Empty>(
     "preempt_teleop", rclcpp::SystemDefaultsQoS(),
@@ -73,7 +77,7 @@ ResultStatus AssistedTeleop::onRun(const std::shared_ptr<const AssistedTeleopAct
 
 void AssistedTeleop::onActionCompletion(std::shared_ptr<AssistedTeleopActionResult>/*result*/)
 {
-  teleop_twist_ = geometry_msgs::msg::Twist();
+  teleop_twist_ = geometry_msgs::msg::TwistStamped();
   preempt_teleop_ = false;
 }
 
@@ -115,11 +119,11 @@ ResultStatus AssistedTeleop::onCycleUpdate()
   projected_pose.y = current_pose.pose.position.y;
   projected_pose.theta = tf2::getYaw(current_pose.pose.orientation);
 
-  geometry_msgs::msg::Twist scaled_twist = teleop_twist_;
+  geometry_msgs::msg::TwistStamped scaled_twist = teleop_twist_;
   for (double time = simulation_time_step_; time < projection_time_;
     time += simulation_time_step_)
   {
-    projected_pose = projectPose(projected_pose, teleop_twist_, simulation_time_step_);
+    projected_pose = projectPose(projected_pose, teleop_twist_.twist, simulation_time_step_);
 
     if (!local_collision_checker_->isCollisionFree(projected_pose)) {
       if (time == simulation_time_step_) {
@@ -128,9 +132,9 @@ ResultStatus AssistedTeleop::onCycleUpdate()
           *clock_,
           1000,
           behavior_name_.c_str() << " collided on first time step, setting velocity to zero");
-        scaled_twist.linear.x = 0.0f;
-        scaled_twist.linear.y = 0.0f;
-        scaled_twist.angular.z = 0.0f;
+        scaled_twist.twist.linear.x = 0.0f;
+        scaled_twist.twist.linear.y = 0.0f;
+        scaled_twist.twist.angular.z = 0.0f;
         break;
       } else {
         RCLCPP_DEBUG_STREAM_THROTTLE(
@@ -139,9 +143,9 @@ ResultStatus AssistedTeleop::onCycleUpdate()
           1000,
           behavior_name_.c_str() << " collision approaching in " << time << " seconds");
         double scale_factor = time / projection_time_;
-        scaled_twist.linear.x *= scale_factor;
-        scaled_twist.linear.y *= scale_factor;
-        scaled_twist.angular.z *= scale_factor;
+        scaled_twist.twist.linear.x *= scale_factor;
+        scaled_twist.twist.linear.y *= scale_factor;
+        scaled_twist.twist.angular.z *= scale_factor;
         break;
       }
     }
@@ -171,11 +175,6 @@ geometry_msgs::msg::Pose2D AssistedTeleop::projectPose(
   return projected_pose;
 }
 
-void AssistedTeleop::teleopVelocityCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
-{
-  teleop_twist_ = *msg;
-}
-
 void AssistedTeleop::preemptTeleopCallback(const std_msgs::msg::Empty::SharedPtr)
 {
   preempt_teleop_ = true;

nav2_behaviors/plugins/spin.cpp

@@ -138,28 +138,28 @@ ResultStatus Spin::onCycleUpdate()
   double vel = sqrt(2 * rotational_acc_lim_ * remaining_yaw);
   vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
 
-  auto cmd_vel = std::make_unique<geometry_msgs::msg::Twist>();
-  cmd_vel->angular.z = copysign(vel, cmd_yaw_);
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  cmd_vel->twist.angular.z = copysign(vel, cmd_yaw_);
 
   geometry_msgs::msg::Pose2D pose2d;
   pose2d.x = current_pose.pose.position.x;
   pose2d.y = current_pose.pose.position.y;
   pose2d.theta = tf2::getYaw(current_pose.pose.orientation);
 
-  if (!isCollisionFree(relative_yaw_, cmd_vel.get(), pose2d)) {
+  if (!isCollisionFree(relative_yaw_, cmd_vel->twist, pose2d)) {
     stopRobot();
     RCLCPP_WARN(logger_, "Collision Ahead - Exiting Spin");
     return ResultStatus{Status::FAILED, SpinActionResult::COLLISION_AHEAD};
   }
 
-  vel_pub_->publish(std::move(cmd_vel));
+  vel_pub_->publish(*cmd_vel);
 
   return ResultStatus{Status::RUNNING, SpinActionResult::NONE};
 }
 
 bool Spin::isCollisionFree(
   const double & relative_yaw,
-  geometry_msgs::msg::Twist * cmd_vel,
+  const geometry_msgs::msg::Twist & cmd_vel,
   geometry_msgs::msg::Pose2D & pose2d)
 {
   // Simulate ahead by simulate_ahead_time_ in cycle_frequency_ increments
@@ -170,7 +170,7 @@ bool Spin::isCollisionFree(
   bool fetch_data = true;
 
   while (cycle_count < max_cycle_count) {
-    sim_position_change = cmd_vel->angular.z * (cycle_count / cycle_frequency_);
+    sim_position_change = cmd_vel.angular.z * (cycle_count / cycle_frequency_);
     pose2d.theta = init_pose.theta + sim_position_change;
     cycle_count++;
 

nav2_collision_monitor/README.md

@@ -88,3 +88,5 @@ The zones around the robot and the data sources are the same as for the Collisio
 ### Configuration
 
 Detailed configuration parameters, their description and how to setup a Collision Detector could be found at its [Configuration Guide](https://navigation.ros.org/configuration/packages/collision_monitor/configuring-collision-detector-node.html).
+
+The `CollisionMonitor` node makes use of a [nav2_util::TwistSubscriber](../nav2_util/README.md#twist-publisher-and-twist-subscriber-for-commanded-velocities).

nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp

@@ -22,12 +22,15 @@
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 
 #include "tf2/time.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
 
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/twist_publisher.hpp"
+#include "nav2_util/twist_subscriber.hpp"
 #include "nav2_msgs/msg/collision_monitor_state.hpp"
 
 #include "nav2_collision_monitor/types.hpp"
@@ -95,7 +98,8 @@ class CollisionMonitor : public nav2_util::LifecycleNode
    * @brief Callback for input cmd_vel
    * @param msg Input cmd_vel message
    */
-  void cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg);
+  void cmdVelInCallbackStamped(geometry_msgs::msg::TwistStamped::ConstSharedPtr msg);
+  void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::ConstSharedPtr msg);
   /**
    * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
    * quit to publish 0-velocity.
@@ -203,10 +207,10 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   std::vector<std::shared_ptr<Source>> sources_;
 
   // Input/output speed controls
-  /// @beirf Input cmd_vel subscriber
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_in_sub_;
+  /// @brief Input cmd_vel subscriber
+  std::shared_ptr<nav2_util::TwistSubscriber> cmd_vel_in_sub_;
   /// @brief Output cmd_vel publisher
-  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_out_pub_;
+  std::unique_ptr<nav2_util::TwistPublisher> cmd_vel_out_pub_;
 
   /// @brief CollisionMonitor state publisher
   rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::CollisionMonitorState>::SharedPtr