Migrating twist to twiststamped in simulations for recommended default bringups (ros-navigation#4779)

* migrating from twist to twiststamped

Signed-off-by: Steve Macenski <[email protected]>

* bump ci cache for updated TB4 sim files

Signed-off-by: Steve Macenski <[email protected]>

* fixing collision monitor, velocity smoother unit tests

Signed-off-by: Steve Macenski <[email protected]>

* fix assisted teleop test

Signed-off-by: Steve Macenski <[email protected]>

* fixing docking server smoke test

Signed-off-by: Steve Macenski <[email protected]>

* bust nav2 minimal tb sim cache

---------

Signed-off-by: Steve Macenski <[email protected]>
Signed-off-by: Joseph Duchesne <[email protected]>

Author: SteveMacenski

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v29\
+              - "<< parameters.key >>-v31\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v29\
+              - "<< parameters.key >>-v31\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v29\
+            key: "<< parameters.key >>-v31\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_collision_monitor/test/collision_monitor_node_test.cpp

@@ -220,6 +220,7 @@ class Tester : public ::testing::Test
 Tester::Tester()
 {
   cm_ = std::make_shared<CollisionMonitorWrapper>();
+  cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
 
   footprint_pub_ = cm_->create_publisher<geometry_msgs::msg::PolygonStamped>(
     FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

nav2_docking/opennav_docking/test/test_docking_server.py

@@ -17,7 +17,7 @@
 import unittest
 
 from action_msgs.msg import GoalStatus
-from geometry_msgs.msg import TransformStamped, Twist
+from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
 from launch import LaunchDescription
 from launch_ros.actions import Node
 import launch_testing
@@ -90,8 +90,8 @@ def tearDownClass(cls):
         rclpy.shutdown()
 
     def command_velocity_callback(self, msg):
-        self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
-        self.command = msg
+        self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z))
+        self.command = msg.twist
 
     def timer_callback(self):
         # Propagate command
@@ -155,7 +155,7 @@ def test_docking_server(self):
 
         # Subscribe to command velocity
         self.node.create_subscription(
-            Twist,
+            TwistStamped,
             'cmd_vel',
             self.command_velocity_callback,
             10

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -74,7 +74,7 @@ def __init__(self):
         self.declare_parameter('scan_frame_id', 'base_scan')
         self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value
 
-        self.declare_parameter('enable_stamped_cmd_vel', False)
+        self.declare_parameter('enable_stamped_cmd_vel', True)
         use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value
 
         self.declare_parameter('scan_publish_dur', 0.1)

nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp

@@ -54,13 +54,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
     node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);
 
   cmd_vel_pub_ =
-    node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
+    node_->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel_teleop", 10);
 
   subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
     std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
 
-  filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
+  filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
     "cmd_vel",
     rclcpp::SystemDefaultsQoS(),
     std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
@@ -167,9 +167,9 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
   counter_ = 0;
   auto start_time = std::chrono::system_clock::now();
   while (rclcpp::ok()) {
-    geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
-    cmd_vel.linear.x = lin_vel;
-    cmd_vel.angular.z = ang_vel;
+    geometry_msgs::msg::TwistStamped cmd_vel = geometry_msgs::msg::TwistStamped();
+    cmd_vel.twist.linear.x = lin_vel;
+    cmd_vel.twist.angular.z = ang_vel;
     cmd_vel_pub_->publish(cmd_vel);
 
     if (counter_ > 1) {
@@ -265,9 +265,9 @@ void AssistedTeleopBehaviorTester::amclPoseCallback(
 }
 
 void AssistedTeleopBehaviorTester::filteredVelCallback(
-  geometry_msgs::msg::Twist::SharedPtr msg)
+  geometry_msgs::msg::TwistStamped::SharedPtr msg)
 {
-  if (msg->linear.x == 0.0f) {
+  if (msg->twist.linear.x == 0.0f) {
     counter_++;
   } else {
     counter_ = 0;

nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp

@@ -24,7 +24,7 @@
 
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
-#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
 #include "nav2_msgs/action/assisted_teleop.hpp"
@@ -68,7 +68,7 @@ class AssistedTeleopBehaviorTester
 
   void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
 
-  void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
+  void filteredVelCallback(geometry_msgs::msg::TwistStamped::SharedPtr msg);
 
   unsigned int counter_;
   bool is_active_;
@@ -83,11 +83,11 @@ class AssistedTeleopBehaviorTester
   // Publishers
   rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
   rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_pub_;
 
   // Subscribers
   rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr filtered_vel_sub_;
 
   // Action client to call AssistedTeleop action
   rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;

nav2_util/include/nav2_util/twist_publisher.hpp

@@ -61,7 +61,7 @@ class TwistPublisher
     using nav2_util::declare_parameter_if_not_declared;
     declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue{false});
+      rclcpp::ParameterValue{true});
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_pub_ = node->create_publisher<geometry_msgs::msg::TwistStamped>(
@@ -122,7 +122,7 @@ class TwistPublisher
 
 protected:
   std::string topic_;
-  bool is_stamped_;
+  bool is_stamped_{true};
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::TwistStamped>::SharedPtr
     twist_stamped_pub_;

nav2_util/include/nav2_util/twist_subscriber.hpp

@@ -92,7 +92,7 @@ class TwistSubscriber
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue(false));
+      rclcpp::ParameterValue(true));
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -125,7 +125,7 @@ class TwistSubscriber
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "enable_stamped_cmd_vel",
-      rclcpp::ParameterValue(false));
+      rclcpp::ParameterValue(true));
     node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
     if (is_stamped_) {
       twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -140,7 +140,7 @@ class TwistSubscriber
 
 protected:
   //! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel
-  bool is_stamped_{false};
+  bool is_stamped_{true};
   //! @brief The subscription when using Twist
   rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_ {nullptr};
   //! @brief The subscription when using TwistStamped

nav2_util/test/test_twist_publisher.cpp

@@ -29,6 +29,7 @@ TEST(TwistPublisher, Unstamped)
   rclcpp::init(0, nullptr);
   auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
   pub_node->configure();
+  pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
   auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
   ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
   EXPECT_FALSE(vel_publisher->is_activated());

nav2_util/test/test_twist_subscriber.cpp

@@ -29,6 +29,7 @@ TEST(TwistSubscriber, Unstamped)
   auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
   sub_node->configure();
   sub_node->activate();
+  sub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
 
   geometry_msgs::msg::TwistStamped sub_msg {};
   auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(