1594 twist stamped publisher (ros-navigation#4077)

* Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <[email protected]> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <[email protected]> * Remove stored node and assert Signed-off-by: Ryan Friedman <[email protected]> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <[email protected]> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <[email protected]> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <[email protected]> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <[email protected]> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <[email protected]> * Fix syntax errors Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <[email protected]> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <[email protected]> * Remove unused code Signed-off-by: Ryan Friedman <[email protected]> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <[email protected]> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <[email protected]> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <[email protected]> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <[email protected]> * Add other constructor and docs Signed-off-by: Ryan Friedman <[email protected]> * Fix linter issues Signed-off-by: Ryan Friedman <[email protected]> * Manually fix paren alignment Signed-off-by: Ryan Friedman <[email protected]> * Remove GSoC reference Signed-off-by: Ryan Friedman <[email protected]> * Document twist bool param in README Signed-off-by: Ryan Friedman <[email protected]> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <[email protected]> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <[email protected]> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <[email protected]> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <[email protected]> * More linter fixes Signed-off-by: Ryan Friedman <[email protected]> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <[email protected]> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <[email protected]> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <[email protected]> --------- Signed-off-by: Ryan Friedman <[email protected]> Signed-off-by: Steve Macenski <[email protected]> Signed-off-by: Rhys Mainwaring <[email protected]> Co-authored-by: pedro-fuoco <[email protected]> Co-authored-by: Steve Macenski <[email protected]> Co-authored-by: Rhys Mainwaring <[email protected]> Signed-off-by: Brice <[email protected]>
BriceRenaudeau · Jan 29, 2024 · 67b9afb · 67b9afb
1 parent 521bff1
commit 67b9afb
Show file tree

Hide file tree

Showing 25 changed files with 697 additions and 111 deletions.
diff --git a/nav2_behaviors/README.md b/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).
diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/assisted_teleop.hpp b/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::unique_ptr<nav2_util::TwistSubscriber> vel_sub_;
   rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr preempt_teleop_sub_;
 
   rclcpp::Duration command_time_allowance_{0, 0};

diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp b/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,17 +126,19 @@ 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->header.stamp = this->clock_->now();
+    cmd_vel->header.frame_id = this->robot_base_frame_;
+    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};
@@ -162,7 +165,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 +177,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++;

diff --git a/nav2_behaviors/include/nav2_behaviors/plugins/spin.hpp b/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_;

diff --git a/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp b/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,10 +290,12 @@ 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;
+    auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+    cmd_vel->header.frame_id = robot_base_frame_;
+    cmd_vel->header.stamp = clock_->now();
+    cmd_vel->twist.linear.x = 0.0;
+    cmd_vel->twist.linear.y = 0.0;
+    cmd_vel->twist.angular.z = 0.0;
 
     vel_pub_->publish(std::move(cmd_vel));
   }

diff --git a/nav2_behaviors/plugins/assisted_teleop.cpp b/nav2_behaviors/plugins/assisted_teleop.cpp
@@ -50,11 +50,14 @@ 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_unique<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 +76,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 +118,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_;
+  auto scaled_twist = std::make_unique<geometry_msgs::msg::TwistStamped>(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 +131,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 +142,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 +174,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;

diff --git a/nav2_behaviors/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp
@@ -138,15 +138,17 @@ 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->header.frame_id = robot_base_frame_;
+  cmd_vel->header.stamp = clock_->now();
+  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};
@@ -159,7 +161,7 @@ ResultStatus Spin::onCycleUpdate()
 
 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 +172,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++;
 

diff --git a/nav2_collision_monitor/README.md b/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).
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/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,13 +98,15 @@ 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::SharedPtr msg);
+  void cmdVelInCallbackUnstamped(geometry_msgs::msg::Twist::SharedPtr msg);
   /**
    * @brief Publishes output cmd_vel. If robot was stopped more than stop_pub_timeout_ seconds,
    * quit to publish 0-velocity.
    * @param robot_action Robot action to publish
+   * @param header TwistStamped header to use
    */
-  void publishVelocity(const Action & robot_action);
+  void publishVelocity(const Action & robot_action, const std_msgs::msg::Header & header);
 
   /**
    * @brief Supporting routine obtaining all ROS-parameters
@@ -145,8 +150,9 @@ class CollisionMonitor : public nav2_util::LifecycleNode
   /**
    * @brief Main processing routine
    * @param cmd_vel_in Input desired robot velocity
+   * @param header Twist header
    */
-  void process(const Velocity & cmd_vel_in);
+  void process(const Velocity & cmd_vel_in, const std_msgs::msg::Header & header);
 
   /**
    * @brief Processes the polygon of STOP, SLOWDOWN and LIMIT action type
@@ -203,10 +209,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::unique_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