From adcbfd6b380141606bfc45fa0bc8d916f48582cf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 12:15:53 -0800 Subject: [PATCH 1/6] migrating from twist to twiststamped Signed-off-by: Steve Macenski --- nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py | 2 +- nav2_util/include/nav2_util/twist_publisher.hpp | 4 ++-- nav2_util/include/nav2_util/twist_subscriber.hpp | 6 +++--- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py b/nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py index 71560c5aa80..4000ddfb16e 100644 --- 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_util/include/nav2_util/twist_publisher.hpp b/nav2_util/include/nav2_util/twist_publisher.hpp index b86fdee5e78..025f3aabe68 100644 --- 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( @@ -122,7 +122,7 @@ class TwistPublisher protected: std::string topic_; - bool is_stamped_; + bool is_stamped_{true}; rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr twist_stamped_pub_; diff --git a/nav2_util/include/nav2_util/twist_subscriber.hpp b/nav2_util/include/nav2_util/twist_subscriber.hpp index 01f9ca25fed..eec5a3e1ae4 100644 --- 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( @@ -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( @@ -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::SharedPtr twist_sub_ {nullptr}; //! @brief The subscription when using TwistStamped From 2d2d4fd64503d9b6da617ef3e0da5fc9098f794e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 12:19:05 -0800 Subject: [PATCH 2/6] bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski --- .circleci/config.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 385950c5f01..40fce70377c 100644 --- 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 >>-v30\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v29\ + - "<< parameters.key >>-v30\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v29\ + key: "<< parameters.key >>-v30\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ From 4a6c6a86eb2d6fa5b56f01480a2939bd56b3f7bc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 14:38:20 -0800 Subject: [PATCH 3/6] fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski --- .../test/collision_monitor_node_test.cpp | 1 + nav2_util/test/test_twist_publisher.cpp | 1 + nav2_util/test/test_twist_subscriber.cpp | 1 + .../test/test_velocity_smoother.cpp | 21 +++++++++++-------- 4 files changed, 15 insertions(+), 9 deletions(-) diff --git a/nav2_collision_monitor/test/collision_monitor_node_test.cpp b/nav2_collision_monitor/test/collision_monitor_node_test.cpp index f297ca363a2..1e8c0bf013a 100644 --- 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(); + cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); footprint_pub_ = cm_->create_publisher( FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); diff --git a/nav2_util/test/test_twist_publisher.cpp b/nav2_util/test/test_twist_publisher.cpp index 94361e94aa8..be185b47f74 100644 --- 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("pub_node", ""); pub_node->configure(); + pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false)); auto vel_publisher = std::make_unique(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 index 56728873682..c18553bc391 100644 --- 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("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( diff --git a/nav2_velocity_smoother/test/test_velocity_smoother.cpp b/nav2_velocity_smoother/test/test_velocity_smoother.cpp index 76c5e0498b6..a5d34149bc4 100644 --- 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(); - cmd->linear.x = 1.0; // Max is 0.5, so should threshold + auto cmd = std::make_shared(); + 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(); - cmd->linear.x = 1.0; // Max is 0.5, so should threshold + auto cmd = std::make_shared(); + 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("cmd_vel", 1); + auto pub = smoother->create_publisher("cmd_vel", 1); pub->on_activate(); - auto msg = std::make_unique(); - msg->linear.x = 100.0; + auto msg = std::make_unique(); + msg->twist.linear.x = 100.0; pub->publish(std::move(msg)); rclcpp::spin_some(smoother->get_node_base_interface()); From df4d394d8ed9e99b1da853f8674ab55a61d38d49 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 14:43:07 -0800 Subject: [PATCH 4/6] fix assisted teleop test Signed-off-by: Steve Macenski --- .../assisted_teleop_behavior_tester.cpp | 14 +++++++------- .../assisted_teleop_behavior_tester.hpp | 8 ++++---- 2 files changed, 11 insertions(+), 11 deletions(-) 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 index bcc1950bde0..f846201cac1 100644 --- 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("preempt_teleop", 10); cmd_vel_pub_ = - node_->create_publisher("cmd_vel_teleop", 10); + node_->create_publisher("cmd_vel_teleop", 10); subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); - filtered_vel_sub_ = node_->create_subscription( + filtered_vel_sub_ = node_->create_subscription( "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 index 37bdd790c2c..626588bbd09 100644 --- 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::SharedPtr initial_pose_pub_; rclcpp::Publisher::SharedPtr preempt_pub_; - rclcpp::Publisher::SharedPtr cmd_vel_pub_; + rclcpp::Publisher::SharedPtr cmd_vel_pub_; // Subscribers rclcpp::Subscription::SharedPtr subscription_; - rclcpp::Subscription::SharedPtr filtered_vel_sub_; + rclcpp::Subscription::SharedPtr filtered_vel_sub_; // Action client to call AssistedTeleop action rclcpp_action::Client::SharedPtr client_ptr_; From 6ea49cd28f274fef6e42200750c885e2d0b2d81f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 14:51:06 -0800 Subject: [PATCH 5/6] fixing docking server smoke test Signed-off-by: Steve Macenski --- nav2_docking/opennav_docking/test/test_docking_server.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_docking/opennav_docking/test/test_docking_server.py b/nav2_docking/opennav_docking/test/test_docking_server.py index 5b76f6095ce..43e3e2df329 100644 --- 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 From 5c5788ffb319db7f938ccf3c8528ba71739728b6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 3 Dec 2024 17:18:38 -0800 Subject: [PATCH 6/6] bust nav2 minimal tb sim cache --- .circleci/config.yml | 6 +++--- tools/underlay.repos | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/.circleci/config.yml b/.circleci/config.yml index 40fce70377c..c52e5a26e57 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v30\ + - "<< parameters.key >>-v31\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v30\ + - "<< parameters.key >>-v31\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v30\ + key: "<< parameters.key >>-v31\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ diff --git a/tools/underlay.repos b/tools/underlay.repos index 09a6da9a92b..005f11ec3f0 100644 --- 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