Migrating twist to twiststamped in simulations for recommended defaul…

…t bringups (#4779) * migrating from twist to twiststamped Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix assisted teleop test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server smoke test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
ros-navigation · Dec 4, 2024 · add09f6 · add09f6
1 parent 9284c8a
commit add09f6
Show file tree

Hide file tree

Showing 12 changed files with 40 additions and 34 deletions.
diff --git a/.circleci/config.yml b/.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 }}\

diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/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());

diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/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

diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/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)

diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp b/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;

diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp b/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_;

diff --git a/nav2_util/include/nav2_util/twist_publisher.hpp b/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_;

diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/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

diff --git a/nav2_util/test/test_twist_publisher.cpp b/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());

diff --git a/nav2_util/test/test_twist_subscriber.cpp b/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>(

diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp
@@ -22,7 +22,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_velocity_smoother/velocity_smoother.hpp"
 #include "nav_msgs/msg/odometry.hpp"
-#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
 #include "nav2_util/twist_subscriber.hpp"
 
 using namespace std::chrono_literals;
@@ -53,7 +53,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
   bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;}
   geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;}
 
-  void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);}
+  void sendCommandMsg(geometry_msgs::msg::TwistStamped::SharedPtr msg)
+  {
+    inputCommandStampedCallback(msg);
+  }
 };
 
 TEST(VelocitySmootherTest, openLoopTestTimer)
@@ -81,8 +84,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
     });
 
   // Send a velocity command
-  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
-  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  cmd->twist.linear.x = 1.0;  // Max is 0.5, so should threshold
   smoother->sendCommandMsg(cmd);
 
   // Process velocity smoothing and send updated odometry based on commands
@@ -147,8 +150,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
   }
 
   // Send a velocity command
-  auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
-  cmd->linear.x = 1.0;  // Max is 0.5, so should threshold
+  auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
+  cmd->twist.linear.x = 1.0;  // Max is 0.5, so should threshold
   smoother->sendCommandMsg(cmd);
 
   // Process velocity smoothing and send updated odometry based on commands
@@ -568,10 +571,10 @@ TEST(VelocitySmootherTest, testCommandCallback)
   smoother->configure(state);
   smoother->activate(state);
 
-  auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+  auto pub = smoother->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 1);
   pub->on_activate();
-  auto msg = std::make_unique<geometry_msgs::msg::Twist>();
-  msg->linear.x = 100.0;
+  auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  msg->twist.linear.x = 100.0;
   pub->publish(std::move(msg));
   rclcpp::spin_some(smoother->get_node_base_interface());
 

diff --git a/tools/underlay.repos b/tools/underlay.repos
@@ -34,4 +34,4 @@ repositories:
   ros-navigation/nav2_minimal_turtlebot_simulation:
     type: git
     url:  https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
-    version: f0eeedbc95d9f7cc8a513f7d46a84b3d08a3d395 
+    version: 091b5ff12436890a54de6325df3573ae110e912b