From 2bd10268602e6edd07e92063717097268745308c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 May 2024 17:55:08 +0000 Subject: [PATCH 1/3] Remove unstamped twist subscribers + parameters --- .../ackermann_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test_ackermann_steering_controller.hpp | 1 - .../bicycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - .../test/test_bicycle_steering_controller.hpp | 1 - .../steering_controllers_library.hpp | 1 - .../src/steering_controllers_library.cpp | 22 ++----- .../src/steering_controllers_library.yaml | 8 --- .../steering_controllers_library_params.yaml | 1 - .../test_steering_controllers_library.hpp | 1 - .../config/tricycle_drive_controller.yaml | 1 - .../tricycle_controller.hpp | 2 - .../src/tricycle_controller.cpp | 63 +++++-------------- .../src/tricycle_controller_parameter.yaml | 5 -- .../test_tricycle_steering_controller.hpp | 1 - .../tricycle_steering_controller_params.yaml | 1 - ...steering_controller_preceeding_params.yaml | 1 - 18 files changed, 23 insertions(+), 90 deletions(-) diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6b64f901c3..6064814d32 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml index ecb1b071e3..5d42adcdd5 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_ackermann_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index a047186d14..49331ae8a2 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -280,7 +280,6 @@ class AckermannSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index a2a6c6508b..3a656cc724 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml index 39ffeed878..e1b9c1ab72 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_bicycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_wheel_joint] diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 5e21ff228c..390447c25f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -251,7 +251,6 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = {rear_wheels_names_[0], front_wheels_names_[0]}; diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index b560e2a782..6055dd69ae 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -98,7 +98,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // Command subscribers and Controller State publisher rclcpp::Subscription::SharedPtr ref_subscriber_twist_ = nullptr; rclcpp::Subscription::SharedPtr ref_subscriber_ackermann_ = nullptr; - rclcpp::Subscription::SharedPtr ref_subscriber_unstamped_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0); // 0ms diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 9bf9fa51d6..8ec4e9d3dc 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -114,22 +114,12 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - if (params_.use_stamped_vel) - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - ref_subscriber_twist_ = get_node()->create_subscription( - "~/reference", subscribers_qos, - std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); - } - else - { - ref_subscriber_unstamped_ = get_node()->create_subscription( - "~/reference_unstamped", subscribers_qos, - std::bind( - &SteeringControllersLibrary::reference_callback_unstamped, this, std::placeholders::_1)); - } + RCLCPP_WARN( + get_node()->get_logger(), + "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); + ref_subscriber_twist_ = get_node()->create_subscription( + "~/reference", subscribers_qos, + std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1)); std::shared_ptr msg = std::make_shared(); diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index b18cac5ae1..711a780458 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -112,11 +112,3 @@ steering_controllers_library: position_feedback is true then ``HW_IF_POSITION`` is taken as interface type", read_only: false, } - - use_stamped_vel: { - type: bool, - default_value: false, - description: "(Deprecated) Choice of vel type, if use_stamped_vel is false then ``geometry_msgs::msg::Twist`` is taken as vel msg type, if - use_stamped_vel is true then ``geometry_msgs::msg::TwistStamped`` is taken as vel msg type", - read_only: false, - } diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index d200d34961..01bfb02302 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,7 +6,6 @@ test_steering_controllers_library: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [front_right_steering_joint, front_left_steering_joint] diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1284b72096..05ac17b454 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -301,7 +301,6 @@ class SteeringControllersLibraryFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index fba6f34bb2..a7853954d7 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -36,7 +36,6 @@ tricycle_controller: # cmd_vel input cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received - use_stamped_vel: false # Set to True if using TwistStamped. # Debug publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index 24aaf89de9..b6f9aae417 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -140,8 +140,6 @@ class TricycleController : public controller_interface::ControllerInterface bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; - rclcpp::Subscription::SharedPtr - velocity_command_unstamped_subscriber_ = nullptr; realtime_tools::RealtimeBox> received_velocity_msg_ptr_{nullptr}; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index 5586b87efc..14d8ddacdb 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -239,7 +239,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; params_.publish_ackermann_command = get_node()->get_parameter("publish_ackermann_command").as_bool(); - params_.use_stamped_vel = get_node()->get_parameter("use_stamped_vel").as_bool(); try { @@ -291,52 +290,25 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & } // initialize command subscriber - if (params_.use_stamped_vel) - { - velocity_command_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + velocity_command_subscriber_ = get_node()->create_subscription( + DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), + [this](const std::shared_ptr msg) -> void + { + if (!subscriber_is_active_) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) - { - RCLCPP_WARN_ONCE( - get_node()->get_logger(), - "Received TwistStamped with zero timestamp, setting it to current " - "time, this message will only be shown once"); - msg->header.stamp = get_node()->get_clock()->now(); - } - received_velocity_msg_ptr_.set(std::move(msg)); - }); - } - else - { - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); - velocity_command_unstamped_subscriber_ = get_node()->create_subscription( - DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(), - [this](const std::shared_ptr msg) -> void + RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); + return; + } + if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0)) { - if (!subscriber_is_active_) - { - RCLCPP_WARN( - get_node()->get_logger(), "Can't accept new commands. subscriber is inactive"); - return; - } - - // Write fake header in the stored stamped command - std::shared_ptr twist_stamped; - received_velocity_msg_ptr_.get(twist_stamped); - twist_stamped->twist = *msg; - twist_stamped->header.stamp = get_node()->get_clock()->now(); - }); - } + RCLCPP_WARN_ONCE( + get_node()->get_logger(), + "Received TwistStamped with zero timestamp, setting it to current " + "time, this message will only be shown once"); + msg->header.stamp = get_node()->get_clock()->now(); + } + received_velocity_msg_ptr_.set(std::move(msg)); + }); // initialize odometry publisher and messasge odometry_publisher_ = get_node()->create_publisher( @@ -460,7 +432,6 @@ bool TricycleController::reset() subscriber_is_active_ = false; velocity_command_subscriber_.reset(); - velocity_command_unstamped_subscriber_.reset(); received_velocity_msg_ptr_.set(nullptr); is_halted = false; diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index b6d1fdf964..cf661eddc1 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -84,11 +84,6 @@ tricycle_controller: gt<>: [0] } } - use_stamped_vel: { - type: bool, - default_value: true, - description: "(deprecated) Use stamp from input velocity message to calculate how old the command actually is.", - } traction: # "The positive limit will be applied to both directions. Setting different limits for positive " # "and negative directions is not supported. Actuators are " diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 6a516691b8..1807ab59a8 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -265,7 +265,6 @@ class TricycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - bool use_stamped_vel_ = true; std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; std::vector front_wheels_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index 6bfb87a892..cc640e1503 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -6,7 +6,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] front_wheels_names: [steering_axis_joint] diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index ea8b88002e..7cb2e4906f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -5,7 +5,6 @@ test_tricycle_steering_controller: open_loop: false velocity_rolling_window_size: 10 position_feedback: false - use_stamped_vel: true rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] front_wheels_names: [pid_controller/steering_axis_joint] rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] From 08ee7d22d851c212511221599b57efd5a0f0edf1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 26 May 2024 17:57:18 +0000 Subject: [PATCH 2/3] Remove reference_callback_unstamped --- .../steering_controllers_library.hpp | 1 - .../src/steering_controllers_library.cpp | 36 ------------------- 2 files changed, 37 deletions(-) diff --git a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index 6055dd69ae..c11d90dc6f 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp @@ -142,7 +142,6 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl // callback for topic interface STEERING_CONTROLLERS__VISIBILITY_LOCAL void reference_callback( const std::shared_ptr msg); - void reference_callback_unstamped(const std::shared_ptr msg); }; } // namespace steering_controllers_library diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 8ec4e9d3dc..40db3dbfc4 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -234,42 +234,6 @@ void SteeringControllersLibrary::reference_callback( } } -void SteeringControllersLibrary::reference_callback_unstamped( - const std::shared_ptr msg) -{ - RCLCPP_WARN( - get_node()->get_logger(), - "Use of Twist message without stamped is deprecated and it will be removed in ROS 2 J-Turtle " - "version. Use '~/reference' topic with 'geometry_msgs::msg::TwistStamped' message type in the " - "future."); - auto twist_stamped = *(input_ref_.readFromNonRT()); - twist_stamped->header.stamp = get_node()->now(); - // if no timestamp provided use current time for command timestamp - if (twist_stamped->header.stamp.sec == 0 && twist_stamped->header.stamp.nanosec == 0u) - { - RCLCPP_WARN( - get_node()->get_logger(), - "Timestamp in header is missing, using current time as command timestamp."); - twist_stamped->header.stamp = get_node()->now(); - } - - const auto age_of_last_command = get_node()->now() - twist_stamped->header.stamp; - - if (ref_timeout_ == rclcpp::Duration::from_seconds(0) || age_of_last_command <= ref_timeout_) - { - twist_stamped->twist = *msg; - } - else - { - RCLCPP_ERROR( - get_node()->get_logger(), - "Received message has timestamp %.10f older for %.10f which is more then allowed timeout " - "(%.4f).", - rclcpp::Time(twist_stamped->header.stamp).seconds(), age_of_last_command.seconds(), - ref_timeout_.seconds()); - } -} - controller_interface::InterfaceConfiguration SteeringControllersLibrary::command_interface_configuration() const { From bfb8922ee6332e74572cb87245fae214f8b71aa1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 May 2024 00:12:53 +0200 Subject: [PATCH 3/3] Remove wrong deprecation note Co-authored-by: Sai Kishor Kothakota --- .../src/steering_controllers_library.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 40db3dbfc4..5880000d17 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -114,9 +114,6 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( // Reference Subscriber ref_timeout_ = rclcpp::Duration::from_seconds(params_.reference_timeout); - RCLCPP_WARN( - get_node()->get_logger(), - "[Deprecated] Using geometry_msgs::msg::Twist instead of TwistStamped is deprecated."); ref_subscriber_twist_ = get_node()->create_subscription( "~/reference", subscribers_qos, std::bind(&SteeringControllersLibrary::reference_callback, this, std::placeholders::_1));