From 4710c6000002b628f0f41f9fb3e00174716e9a4c Mon Sep 17 00:00:00 2001 From: Quique Llorente Date: Mon, 6 May 2024 11:47:39 +0200 Subject: [PATCH 01/10] Remove front_steering from steering library To Accommodate controllers that are not only steering at front or rear this change remove the `front_steering` variable from steering_controller_library, as a byproduct of that the notion of front or rear wheel radius is also removed from dependant controllers and the library has know "traction_joints_names" and "steering_joints_names" instead of "front_wheels_names" and "rear_wheels_names". Signed-off-by: Quique Llorente --- .../src/ackermann_steering_controller.cpp | 32 +++-- .../src/ackermann_steering_controller.yaml | 34 +++-- .../ackermann_steering_controller_params.yaml | 11 +- ...steering_controller_preceeding_params.yaml | 15 +-- .../test_ackermann_steering_controller.cpp | 27 ++-- .../test_ackermann_steering_controller.hpp | 39 +++--- ...kermann_steering_controller_preceeding.cpp | 31 ++--- .../src/bicycle_steering_controller.cpp | 18 +-- .../src/bicycle_steering_controller.yaml | 19 ++- .../bicycle_steering_controller_params.yaml | 8 +- ...steering_controller_preceeding_params.yaml | 12 +- .../test/test_bicycle_steering_controller.cpp | 17 ++- .../test/test_bicycle_steering_controller.hpp | 26 ++-- ...bicycle_steering_controller_preceeding.cpp | 20 ++- steering_controllers_library/doc/userdoc.rst | 16 +-- .../steering_controllers_library.hpp | 5 +- .../src/steering_controllers_library.cpp | 124 ++++++------------ .../src/steering_controllers_library.yaml | 33 +++-- .../steering_controllers_library_params.yaml | 5 +- .../test_steering_controllers_library.cpp | 16 +-- .../test_steering_controllers_library.hpp | 47 +++---- .../src/tricycle_steering_controller.cpp | 21 +-- .../src/tricycle_steering_controller.yaml | 19 ++- .../test_tricycle_steering_controller.cpp | 20 ++- .../test_tricycle_steering_controller.hpp | 31 +++-- ...ricycle_steering_controller_preceeding.cpp | 24 ++-- .../tricycle_steering_controller_params.yaml | 8 +- ...steering_controller_preceeding_params.yaml | 12 +- 28 files changed, 330 insertions(+), 360 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..e8eeb9f7b1 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -31,21 +31,35 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { ackermann_params_ = ackermann_param_listener_->get_params(); - const double front_wheels_radius = ackermann_params_.front_wheels_radius; - const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; - const double front_wheel_track = ackermann_params_.front_wheel_track; - const double rear_wheel_track = ackermann_params_.rear_wheel_track; - const double wheelbase = ackermann_params_.wheelbase; + if (ackermann_params_.front_wheels_radius > 0.0) + { + fprintf(stderr, "DEPRECATED parameter 'front_wheels_radius'\n"); + return controller_interface::CallbackReturn::ERROR; + } - if (params_.front_steering) + if (ackermann_params_.rear_wheels_radius > 0.0) { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + fprintf(stderr, "DEPRECATED parameter 'rear_wheels_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } - else + + if (ackermann_params_.front_wheel_track > 0.0) { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + fprintf(stderr, "DEPRECATED parameter 'front_wheel_track'\n"); + return controller_interface::CallbackReturn::ERROR; } + if (ackermann_params_.rear_wheel_track > 0.0) + { + fprintf(stderr, "DEPRECATED parameter 'rear_wheel_track'\n"); + return controller_interface::CallbackReturn::ERROR; + } + + const double traction_wheels_radius = ackermann_params_.traction_wheels_radius; + const double traction_wheel_track = ackermann_params_.traction_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; + + odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_wheel_track); odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 1ec0b41c9f..c130856fa4 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,24 +1,27 @@ ackermann_steering_controller: - front_wheel_track: + traction_wheel_track: { type: double, default_value: 0.0, - description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, validation: { gt<>: [0.0] } } - + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "DEPRECATED: use 'traction_wheel_track'", + read_only: false, + } rear_wheel_track: { type: double, default_value: 0.0, - description: "Rear wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + description: "DEPRECATED: use 'traction_wheel_track'", read_only: false, - validation: { - gt<>: [0.0] - } } wheelbase: @@ -32,24 +35,29 @@ ackermann_steering_controller: } } - front_wheels_radius: + traction_wheels_radius: { type: double, default_value: 0.0, - description: "Front wheels radius.", + description: "Traction wheels radius.", read_only: false, validation: { gt<>: [0.0] } } + front_wheels_radius: + { + type: double, + default_value: 0.0, + description: "DEPRECATED: use 'traction_wheels_radius'", + read_only: false, + } + rear_wheels_radius: { type: double, default_value: 0.0, - description: "Rear wheels radius.", + description: "DEPRECATED: use 'traction_wheels_radius'", read_only: false, - validation: { - gt<>: [0.0] - } } diff --git a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml index 6064814d32..f76b125d56 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_params.yaml @@ -2,15 +2,12 @@ test_ackermann_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheel_track: 1.76868 + traction_wheels_radius: 0.45 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 5d42adcdd5..b0b3f74c65 100644 --- a/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml +++ b/ackermann_steering_controller/test/ackermann_steering_controller_preceeding_params.yaml @@ -1,16 +1,13 @@ test_ackermann_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - 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] - front_wheels_state_names: [front_right_steering_joint, front_left_steering_joint] + traction_joints_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + steering_joints_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + traction_joints_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_state_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheel_track: 1.76868 + traction_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index c04e87be95..b382f314a7 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -30,18 +30,15 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); - ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); - ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); - ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.traction_wheel_track, traction_wheel_track_); } TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) @@ -54,32 +51,32 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + steering_joints_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_RIGHT_WHEEL], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_LEFT_WHEEL], - controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 59637a072f..656ceef376 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -151,22 +151,22 @@ class AckermannSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, + traction_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); @@ -175,22 +175,22 @@ class AckermannSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, + traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -269,29 +269,28 @@ class AckermannSteeringControllerFixture : public ::testing::Test protected: // Controller-related parameters double reference_timeout_ = 2.0; - bool front_steering_ = true; bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = { + std::vector traction_joints_names_ = { + "rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steering_joints_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; std::vector joint_names_ = { - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0], + steering_joints_names_[1]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector wheels_preceeding_names_ = { "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = { + std::vector steers_preceeding_names_ = { "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; std::vector preceeding_joint_names_ = { - rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], - front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; + wheels_preceeding_names_[0], wheels_preceeding_names_[1], steers_preceeding_names_[0], + steers_preceeding_names_[1]}; double wheelbase_ = 3.24644; - double front_wheel_track_ = 2.12321; - double rear_wheel_track_ = 1.76868; - double front_wheels_radius_ = 0.45; - double rear_wheels_radius_ = 0.45; + double traction_wheel_track_ = 1.76868; + double traction_wheels_radius_ = 0.45; std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp index 96dd20d80e..224f0a7011 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -30,20 +30,17 @@ TEST_F(AckermannSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, - testing::ElementsAreArray(rear_wheels_preceeding_names_)); + controller_->params_.traction_joints_names, + testing::ElementsAreArray(wheels_preceeding_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, - testing::ElementsAreArray(front_wheels_preceeding_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, + testing::ElementsAreArray(steers_preceeding_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->ackermann_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->ackermann_params_.front_wheels_radius, front_wheels_radius_); - ASSERT_EQ(controller_->ackermann_params_.rear_wheels_radius, rear_wheels_radius_); - ASSERT_EQ(controller_->ackermann_params_.front_wheel_track, front_wheel_track_); - ASSERT_EQ(controller_->ackermann_params_.rear_wheel_track, rear_wheel_track_); + ASSERT_EQ(controller_->ackermann_params_.traction_wheels_radius, traction_wheels_radius_); + ASSERT_EQ(controller_->ackermann_params_.traction_wheel_track, traction_wheel_track_); } TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) @@ -56,32 +53,32 @@ TEST_F(AckermannSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + preceeding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[1] + "/" + steering_interface_name_); + preceeding_prefix_ + "/" + steering_joints_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_RIGHT_WHEEL], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_LEFT_WHEEL], - controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 95eaf1965c..3f0f663c49 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -31,19 +31,21 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet { bicycle_params_ = bicycle_param_listener_->get_params(); - const double wheelbase = bicycle_params_.wheelbase; - const double front_wheel_radius = bicycle_params_.front_wheel_radius; - const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; - - if (params_.front_steering) + if (bicycle_params_.front_wheel_radius > 0.0) { - odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } - else + + if (bicycle_params_.rear_wheel_radius > 0.0) { - odometry_.set_wheel_params(front_wheel_radius, wheelbase); + fprintf(stderr, "DEPRECATED parameter 'rear_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } + const double wheelbase = bicycle_params_.wheelbase; + const double traction_wheel_radius = bicycle_params_.traction_wheel_radius; + odometry_.set_wheel_params(traction_wheel_radius, wheelbase); odometry_.set_odometry_type(steering_odometry::BICYCLE_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index fde323ef74..5af439ab70 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -10,24 +10,29 @@ bicycle_steering_controller: } } - front_wheel_radius: + traction_wheel_radius: { type: double, default_value: 0.0, - description: "Front wheel radius.", + description: "Traction wheel radius.", read_only: false, validation: { gt<>: [0.0] } } + front_wheel_radius: + { + type: double, + default_value: 0.0, + description: "DEPRECATED: Use 'traction_wheel_radius'", + read_only: true, + } + rear_wheel_radius: { type: double, default_value: 0.0, - description: "Rear wheel radius.", - read_only: false, - validation: { - gt<>: [0.0] - } + description: "DEPRECATED: Use 'traction_wheel_radius'", + read_only: true, } diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index 3a656cc724..530567e721 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml @@ -2,13 +2,11 @@ test_bicycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [rear_wheel_joint] - front_wheels_names: [steering_axis_joint] + traction_joints_names: [rear_wheel_joint] + steering_joints_names: [steering_axis_joint] wheelbase: 3.24644 - front_wheel_radius: 0.45 - rear_wheel_radius: 0.45 + traction_wheel_radius: 0.45 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 e1b9c1ab72..6740a1705b 100644 --- a/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml +++ b/bicycle_steering_controller/test/bicycle_steering_controller_preceeding_params.yaml @@ -1,15 +1,13 @@ test_bicycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [pid_controller/rear_wheel_joint] - front_wheels_names: [pid_controller/steering_axis_joint] - rear_wheels_state_names: [rear_wheel_joint] - front_wheels_state_names: [steering_axis_joint] + traction_joints_names: [pid_controller/rear_wheel_joint] + steering_joints_names: [pid_controller/steering_axis_joint] + traction_joints_state_names: [rear_wheel_joint] + steering_joints_state_names: [steering_axis_joint] wheelbase: 3.24644 - front_wheel_radius: 0.45 - rear_wheel_radius: 0.45 + traction_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 573992b24e..b0e3d49997 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -30,16 +30,14 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); - ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.traction_wheel_radius, traction_wheel_radius_); } TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) @@ -51,19 +49,20 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) auto cmd_if_conf = controller_->command_interface_configuration(); ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( - cmd_if_conf.names[CMD_TRACTION_WHEEL], rear_wheels_names_[0] + "/" + traction_interface_name_); + cmd_if_conf.names[CMD_TRACTION_WHEEL], + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( - cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_AXIS], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 65f1691a3b..878fbcbbd7 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -149,11 +149,13 @@ class BicycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_WHEEL])); + traction_joints_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + steering_joints_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -161,11 +163,12 @@ class BicycleSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_WHEEL])); + traction_joints_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -247,18 +250,17 @@ class BicycleSteeringControllerFixture : public ::testing::Test bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - 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]}; + std::vector traction_joints_names_ = {"rear_wheel_joint"}; + std::vector steering_joints_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {traction_joints_names_[0], steering_joints_names_[0]}; - std::vector rear_wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector wheels_preceeding_names_ = {"pid_controller/rear_wheel_joint"}; + std::vector steers_preceeding_names_ = {"pid_controller/steering_axis_joint"}; std::vector preceeding_joint_names_ = { - rear_wheels_preceeding_names_[0], front_wheels_preceeding_names_[0]}; + wheels_preceeding_names_[0], steers_preceeding_names_[0]}; double wheelbase_ = 3.24644; - double front_wheels_radius_ = 0.45; - double rear_wheels_radius_ = 0.45; + double traction_wheel_radius_ = 0.45; std::array joint_state_values_ = {3.3, 0.5}; std::array joint_command_values_ = {1.1, 2.2}; diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp index 0bc03f4886..1b4a7fcc7c 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -30,18 +30,16 @@ TEST_F(BicycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, - testing::ElementsAreArray(rear_wheels_preceeding_names_)); + controller_->params_.traction_joints_names, + testing::ElementsAreArray(wheels_preceeding_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, - testing::ElementsAreArray(front_wheels_preceeding_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, + testing::ElementsAreArray(steers_preceeding_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->bicycle_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->bicycle_params_.front_wheel_radius, front_wheels_radius_); - ASSERT_EQ(controller_->bicycle_params_.rear_wheel_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->bicycle_params_.traction_wheel_radius, traction_wheel_radius_); } TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) @@ -54,20 +52,20 @@ TEST_F(BicycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + preceeding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_AXIS], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 44b180162e..33887026e9 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -65,13 +65,13 @@ Command interfaces If parameter ``front_steering == true`` -- ``/position`` double, in rad -- ``/velocity`` double, in m/s +- ``/position`` double, in rad +- ``/velocity`` double, in m/s If parameter ``front_steering == false`` -- ``/velocity`` double, in m/s -- ``/position`` double, in rad +- ``/velocity`` double, in m/s +- ``/position`` double, in rad State interfaces ,,,,,,,,,,,,,,,,, @@ -83,13 +83,13 @@ Depending on the ``position_feedback``, different feedback types are expected If parameter ``front_steering == true`` -- ``/position`` double, in rad -- ``/`` double, in m or m/s +- ``/position`` double, in rad +- ``/`` double, in m or m/s If parameter ``front_steering == false`` -- ``/`` double, in m or m/s -- ``/position`` double, in rad +- ``/`` double, in m or m/s +- ``/position`` double, in rad Subscribers ,,,,,,,,,,,, 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 84a892d79e..12ac83f1a7 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 @@ -35,7 +35,6 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" - namespace steering_controllers_library { class SteeringControllersLibrary : public controller_interface::ChainableControllerInterface @@ -129,8 +128,8 @@ class SteeringControllersLibrary : public controller_interface::ChainableControl double last_linear_velocity_ = 0.0; double last_angular_velocity_ = 0.0; - std::vector rear_wheels_state_names_; - std::vector front_wheels_state_names_; + std::vector traction_joints_state_names_; + std::vector steering_joints_state_names_; private: // callback for topic interface diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index f193098f82..d90eadb1f3 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -81,26 +81,39 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); + + if (params_.front_wheels_names.size() > 0) + { + fprintf(stderr, "DEPRECATED parameter 'front_wheels_names'\n"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.rear_wheels_names.size() > 0) + { + fprintf(stderr, "DEPRECATED parameter 'rear_wheels_names'\n"); + return controller_interface::CallbackReturn::ERROR; + } + odometry_.set_velocity_rolling_window_size(params_.velocity_rolling_window_size); configure_odometry(); - if (!params_.rear_wheels_state_names.empty()) + if (!params_.traction_joints_state_names.empty()) { - rear_wheels_state_names_ = params_.rear_wheels_state_names; + traction_joints_state_names_ = params_.traction_joints_state_names; } else { - rear_wheels_state_names_ = params_.rear_wheels_names; + traction_joints_state_names_ = params_.traction_joints_names; } - if (!params_.front_wheels_state_names.empty()) + if (!params_.steering_joints_state_names.empty()) { - front_wheels_state_names_ = params_.front_wheels_state_names; + steering_joints_state_names_ = params_.steering_joints_state_names; } else { - front_wheels_state_names_ = params_.front_wheels_names; + steering_joints_state_names_ = params_.steering_joints_names; } // topics QoS @@ -233,34 +246,16 @@ SteeringControllersLibrary::command_interface_configuration() const controller_interface::InterfaceConfiguration command_interfaces_config; command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; command_interfaces_config.names.reserve(nr_cmd_itfs_); - - if (params_.front_steering) + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); - } - - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); - } + command_interfaces_config.names.push_back( + params_.traction_joints_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); } - else - { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.front_wheels_names[i] + "/" + hardware_interface::HW_IF_VELOCITY); - } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_config.names.push_back( - params_.rear_wheels_names[i] + "/" + hardware_interface::HW_IF_POSITION); - } + for (size_t i = 0; i < params_.steering_joints_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.steering_joints_names[i] + "/" + hardware_interface::HW_IF_POSITION); } return command_interfaces_config; } @@ -275,33 +270,17 @@ SteeringControllersLibrary::state_interface_configuration() const const auto traction_wheels_feedback = params_.position_feedback ? hardware_interface::HW_IF_POSITION : hardware_interface::HW_IF_VELOCITY; - if (params_.front_steering) - { - for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) - { - state_interfaces_config.names.push_back( - rear_wheels_state_names_[i] + "/" + traction_wheels_feedback); - } - for (size_t i = 0; i < front_wheels_state_names_.size(); i++) - { - state_interfaces_config.names.push_back( - front_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); - } - } - else + for (size_t i = 0; i < traction_joints_state_names_.size(); i++) { - for (size_t i = 0; i < front_wheels_state_names_.size(); i++) - { - state_interfaces_config.names.push_back( - front_wheels_state_names_[i] + "/" + traction_wheels_feedback); - } + state_interfaces_config.names.push_back( + traction_joints_state_names_[i] + "/" + traction_wheels_feedback); + } - for (size_t i = 0; i < rear_wheels_state_names_.size(); i++) - { - state_interfaces_config.names.push_back( - rear_wheels_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); - } + for (size_t i = 0; i < steering_joints_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + steering_joints_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } return state_interfaces_config; @@ -398,30 +377,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_, params_.open_loop); - if (params_.front_steering) + for (size_t i = 0; i < params_.traction_joints_names.size(); i++) { - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(traction_commands[i]); - } - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i + params_.rear_wheels_names.size()].set_value(steering_commands[i]); - } + command_interfaces_[i].set_value(traction_commands[i]); } - else + for (size_t i = 0; i < params_.steering_joints_names.size(); i++) { - { - for (size_t i = 0; i < params_.front_wheels_names.size(); i++) - { - command_interfaces_[i].set_value(traction_commands[i]); - } - for (size_t i = 0; i < params_.rear_wheels_names.size(); i++) - { - command_interfaces_[i + params_.front_wheels_names.size()].set_value( - steering_commands[i]); - } - } + command_interfaces_[i + params_.traction_joints_names.size()].set_value(steering_commands[i]); } } @@ -464,14 +426,8 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c controller_state_publisher_->msg_.steer_positions.clear(); controller_state_publisher_->msg_.steering_angle_command.clear(); - auto number_of_traction_wheels = params_.rear_wheels_names.size(); - auto number_of_steering_wheels = params_.front_wheels_names.size(); - - if (!params_.front_steering) - { - number_of_traction_wheels = params_.front_wheels_names.size(); - number_of_steering_wheels = params_.rear_wheels_names.size(); - } + auto number_of_traction_wheels = params_.traction_joints_names.size(); + auto number_of_steering_wheels = params_.steering_joints_names.size(); for (size_t i = 0; i < number_of_traction_wheels; ++i) { diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 711a780458..3082fb3d9b 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -4,17 +4,30 @@ steering_controllers_library: default_value: 1.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } - front_steering: { type: bool, - default_value: true, - description: "Is the steering on the front of the robot?", read_only: true, + default_value: false, + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" + } + + front_wheels_names: { + type: string_array, + read_only: true, + default_value: [], + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" } rear_wheels_names: { type: string_array, - description: "Names of rear wheel joints.", + read_only: true, + default_value: [], + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" + } + + traction_joints_names: { + type: string_array, + description: "Names of wheel joints.", read_only: true, validation: { size_lt<>: [5], @@ -23,9 +36,9 @@ steering_controllers_library: } } - front_wheels_names: { + steering_joints_names: { type: string_array, - description: "Names of front wheel joints.", + description: "Names of steering joints.", read_only: true, validation: { size_lt<>: [5], @@ -34,9 +47,9 @@ steering_controllers_library: } } - rear_wheels_state_names: { + traction_joints_state_names: { type: string_array, - description: "(Optional) Names of rear wheel joints to read states from. If not set joint names from 'rear_wheels_names' will be used.", + description: "(Optional) Names of tractions joints to read states from. If not set joint names from 'traction_joints_names' will be used.", default_value: [], read_only: true, validation: { @@ -45,9 +58,9 @@ steering_controllers_library: } } - front_wheels_state_names: { + steering_joints_state_names: { type: string_array, - description: "(Optional) Names of front wheel joints to read states from. If not set joint names from 'front_wheels_names' will be used.", + description: "(Optional) Names of steering joints to read states from. If not set joint names from 'steering_joints_names' will be used.", default_value: [], read_only: true, validation: { diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index 01bfb02302..26e9330630 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -2,12 +2,11 @@ test_steering_controllers_library: ros__parameters: reference_timeout: 0.1 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [front_right_steering_joint, front_left_steering_joint] + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 front_wheel_track: 2.12321 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index fca7d00946..99ac44e8ae 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -35,32 +35,32 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + steering_joints_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_RIGHT_WHEEL], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_LEFT_WHEEL], - controller_->front_wheels_state_names_[1] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 93ee823e0f..4b63c5db75 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -55,10 +55,8 @@ static constexpr size_t NR_CMD_ITFS = 4; static constexpr size_t NR_REF_ITFS = 2; static constexpr double WHEELBASE_ = 3.24644; -static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; -static constexpr double REAR_WHEEL_TRACK_ = 1.76868; -static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; -static constexpr double REAR_WHEELS_RADIUS_ = 0.45; +static constexpr double WHEELS_TRACK_ = 2.12321; +static constexpr double WHEELS_RADIUS_ = 0.45; namespace { @@ -121,7 +119,7 @@ class TestableSteeringControllersLibrary controller_interface::CallbackReturn configure_odometry() { set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); - odometry_.set_wheel_params(FRONT_WHEELS_RADIUS_, WHEELBASE_, REAR_WHEEL_TRACK_); + odometry_.set_wheel_params(WHEELS_RADIUS_, WHEELBASE_, WHEELS_TRACK_); odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); return controller_interface::CallbackReturn::SUCCESS; @@ -172,22 +170,22 @@ class SteeringControllersLibraryFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, + traction_joints_names_[1], traction_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_STEER_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); @@ -196,22 +194,22 @@ class SteeringControllersLibraryFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, + traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, + steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[1], steering_interface_name_, + steering_joints_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -290,29 +288,24 @@ class SteeringControllersLibraryFixture : public ::testing::Test protected: // Controller-related parameters double reference_timeout_ = 2.0; - bool front_steering_ = true; bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = { + std::vector traction_joints_names_ = { + "rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steering_joints_names_ = { "front_right_steering_joint", "front_left_steering_joint"}; std::vector joint_names_ = { - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0], front_wheels_names_[1]}; + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0], + steering_joints_names_[1]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector wheels_preceeding_names_ = { "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = { + std::vector steers_preceeding_names_ = { "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint"}; std::vector preceeding_joint_names_ = { - rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], - front_wheels_preceeding_names_[0], front_wheels_preceeding_names_[1]}; - - double wheelbase_ = 3.24644; - double front_wheel_track_ = 2.12321; - double rear_wheel_track_ = 1.76868; - double front_wheels_radius_ = 0.45; - double rear_wheels_radius_ = 0.45; + wheels_preceeding_names_[0], wheels_preceeding_names_[1], steers_preceeding_names_[0], + steers_preceeding_names_[1]}; std::array joint_state_values_ = {0.5, 0.5, 0.0, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4}; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..858af82f46 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -30,20 +30,23 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { tricycle_params_ = tricycle_param_listener_->get_params(); - const double front_wheels_radius = tricycle_params_.front_wheels_radius; - const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; - const double wheel_track = tricycle_params_.wheel_track; - const double wheelbase = tricycle_params_.wheelbase; - - if (params_.front_steering) + if (tricycle_params_.front_wheel_radius > 0.0) { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } - else + + if (tricycle_params_.rear_wheel_radius > 0.0) { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + fprintf(stderr, "DEPRECATED parameter 'rear_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } + const double traction_wheels_radius = tricycle_params_.traction_wheels_radius; + const double wheel_track = tricycle_params_.wheel_track; + const double wheelbase = tricycle_params_.wheelbase; + + odometry_.set_wheel_params(traction_wheels_radius, wheelbase, wheel_track); odometry_.set_odometry_type(steering_odometry::TRICYCLE_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 6e5ae2b477..4593389b39 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -21,24 +21,29 @@ tricycle_steering_controller: } } - front_wheels_radius: + traction_wheels_radius: { type: double, default_value: 0.0, - description: "Front wheels radius.", + description: "Traction wheels radius.", read_only: false, validation: { gt<>: [0.0] } } - rear_wheels_radius: + front_wheel_radius: { type: double, default_value: 0.0, - description: "Rear wheels radius.", + description: "DEPRECATED: Use 'traction_wheels_radius'", + read_only: false, + } + + rear_wheel_radius: + { + type: double, + default_value: 0.0, + description: "DEPRECATED: Use 'traction_wheels_radius'", read_only: false, - validation: { - gt<>: [0.0] - } } diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 3f2589cb6c..aa6479593a 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -30,16 +30,14 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, testing::ElementsAreArray(rear_wheels_names_)); + controller_->params_.traction_joints_names, testing::ElementsAreArray(traction_joints_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, testing::ElementsAreArray(steering_joints_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); - ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.traction_wheels_radius, traction_wheels_radius_); ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } @@ -53,25 +51,25 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - rear_wheels_names_[0] + "/" + traction_interface_name_); + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( - cmd_if_conf.names[CMD_STEER_WHEEL], front_wheels_names_[0] + "/" + steering_interface_name_); + cmd_if_conf.names[CMD_STEER_WHEEL], steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_AXIS], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 3b7a053937..712eed8b6f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -150,17 +150,18 @@ class TricycleSteeringControllerFixture : public ::testing::Test command_ifs.reserve(joint_command_values_.size()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_command_values_[CMD_TRACTION_RIGHT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - rear_wheels_names_[1], steering_interface_name_, + traction_joints_names_[1], steering_interface_name_, &joint_command_values_[CMD_TRACTION_LEFT_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); command_itfs_.emplace_back(hardware_interface::CommandInterface( - front_wheels_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); + steering_joints_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -168,17 +169,17 @@ class TricycleSteeringControllerFixture : public ::testing::Test state_ifs.reserve(joint_state_values_.size()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[0], traction_interface_name_, + traction_joints_names_[0], traction_interface_name_, &joint_state_values_[STATE_TRACTION_RIGHT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - rear_wheels_names_[1], traction_interface_name_, + traction_joints_names_[1], traction_interface_name_, &joint_state_values_[STATE_TRACTION_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); state_itfs_.emplace_back(hardware_interface::StateInterface( - front_wheels_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); + steering_joints_names_[0], steering_interface_name_, &joint_state_values_[STATE_STEER_AXIS])); state_ifs.emplace_back(state_itfs_.back()); controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); @@ -256,27 +257,25 @@ class TricycleSteeringControllerFixture : public ::testing::Test protected: // Controller-related parameters double reference_timeout_ = 2.0; - bool front_steering_ = true; bool open_loop_ = false; unsigned int velocity_rolling_window_size_ = 10; bool position_feedback_ = false; - std::vector rear_wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; - std::vector front_wheels_names_ = {"steering_axis_joint"}; + std::vector traction_joints_names_ = { + "rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steering_joints_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + traction_joints_names_[0], traction_joints_names_[1], steering_joints_names_[0]}; - std::vector rear_wheels_preceeding_names_ = { + std::vector wheels_preceeding_names_ = { "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; - std::vector front_wheels_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector steers_preceeding_names_ = {"pid_controller/steering_axis_joint"}; std::vector preceeding_joint_names_ = { - rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], - front_wheels_preceeding_names_[0]}; + wheels_preceeding_names_[0], wheels_preceeding_names_[1], steers_preceeding_names_[0]}; double wheelbase_ = 3.24644; double wheel_track_ = 1.212121; - double front_wheels_radius_ = 0.45; - double rear_wheels_radius_ = 0.45; + double traction_wheels_radius_ = 0.45; std::array joint_state_values_ = {0.5, 0.5, 0.0}; std::array joint_command_values_ = {1.1, 3.3, 2.2}; diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp index 2170659ee7..c3c0f8a3f2 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -30,18 +30,16 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, - testing::ElementsAreArray(rear_wheels_preceeding_names_)); + controller_->params_.traction_joints_names, + testing::ElementsAreArray(wheels_preceeding_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, - testing::ElementsAreArray(front_wheels_preceeding_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, + testing::ElementsAreArray(steers_preceeding_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); - ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.traction_wheels_radius, traction_wheels_radius_); ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } @@ -55,26 +53,26 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + preceeding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_AXIS], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index cc640e1503..fe12ae5feb 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -2,14 +2,12 @@ test_tricycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [steering_axis_joint] + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [steering_axis_joint] wheelbase: 3.24644 wheel_track: 1.212121 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheels_radius: 0.45 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 7cb2e4906f..813a961f8f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -1,15 +1,13 @@ test_tricycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - 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] - front_wheels_state_names: [steering_axis_joint] + traction_joints_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + steering_joints_names: [pid_controller/steering_axis_joint] + traction_joints_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_state_names: [steering_axis_joint] wheelbase: 3.24644 wheel_track: 1.212121 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheels_radius: 0.45 From 75fde5e426efbc6fdb4bc670f48599e9cdc54a77 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:07:38 +0000 Subject: [PATCH 02/10] Mark overriding functions --- .../test/test_steering_controllers_library.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 1a7e6f4d54..8034240b68 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -111,12 +111,12 @@ class TestableSteeringControllersLibrary } // implementing methods which are declared virtual in the steering_controllers_library.hpp - void initialize_implementation_parameter_listener() + void initialize_implementation_parameter_listener() override { param_listener_ = std::make_shared(get_node()); } - controller_interface::CallbackReturn configure_odometry() + controller_interface::CallbackReturn configure_odometry() override { set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); odometry_.set_wheel_params(WHEELS_RADIUS_, WHEELBASE_, WHEELS_TRACK_); @@ -125,7 +125,7 @@ class TestableSteeringControllersLibrary return controller_interface::CallbackReturn::SUCCESS; } - bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } + bool update_odometry(const rclcpp::Duration & /*period*/) override { return true; } }; // We are using template class here for easier reuse of Fixture in specializations of controllers From 76962b58479f303968fc0845fc3a22ea8f064870 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:08:57 +0000 Subject: [PATCH 03/10] Add proper deprecation warnings and backwards compatibility --- .../src/ackermann_steering_controller.cpp | 24 ++++--- .../src/ackermann_steering_controller.yaml | 14 ++-- .../src/bicycle_steering_controller.cpp | 12 ++-- .../src/bicycle_steering_controller.yaml | 7 +- .../src/steering_controllers_library.cpp | 71 +++++++++++++++++-- .../src/steering_controllers_library.yaml | 36 ++++++++-- .../src/tricycle_steering_controller.cpp | 12 ++-- .../src/tricycle_steering_controller.yaml | 7 +- 8 files changed, 146 insertions(+), 37 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 702046d82b..a614723654 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -33,26 +33,34 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom if (ackermann_params_.front_wheels_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'front_wheels_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead"); + ackermann_params_.traction_wheels_radius = ackermann_params_.front_wheels_radius; } if (ackermann_params_.rear_wheels_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'rear_wheels_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead"); + ackermann_params_.traction_wheels_radius = ackermann_params_.rear_wheels_radius; } if (ackermann_params_.front_wheel_track > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'front_wheel_track'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_track', set 'traction_wheel_track' instead"); + ackermann_params_.traction_wheel_track = ackermann_params_.front_wheel_track; } if (ackermann_params_.rear_wheel_track > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'rear_wheel_track'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_track', set 'traction_wheel_track' instead"); + ackermann_params_.traction_wheel_track = ackermann_params_.rear_wheel_track; } const double traction_wheels_radius = ackermann_params_.traction_wheels_radius; diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index c130856fa4..dcd0d8b3e5 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -5,9 +5,10 @@ ackermann_steering_controller: default_value: 0.0, description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, - validation: { - gt<>: [0.0] - } + # TODO(anyone): activate validation if front/rear wheel track is removed + # validation: { + # gt<>: [0.0] + # } } front_wheel_track: { @@ -41,9 +42,10 @@ ackermann_steering_controller: default_value: 0.0, description: "Traction wheels radius.", read_only: false, - validation: { - gt<>: [0.0] - } + # TODO(anyone): activate validation if front/rear wheel radius is removed + # validation: { + # gt<>: [0.0] + # } } front_wheels_radius: diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index 3f0f663c49..b8bd2679c6 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -33,14 +33,18 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet if (bicycle_params_.front_wheel_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheel_radius' instead"); + bicycle_params_.traction_wheel_radius = bicycle_params_.front_wheel_radius; } if (bicycle_params_.rear_wheel_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'rear_wheel_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheel_radius' instead"); + bicycle_params_.traction_wheel_radius = bicycle_params_.rear_wheel_radius; } const double wheelbase = bicycle_params_.wheelbase; const double traction_wheel_radius = bicycle_params_.traction_wheel_radius; diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.yaml b/bicycle_steering_controller/src/bicycle_steering_controller.yaml index 5af439ab70..4ce224cca8 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -16,9 +16,10 @@ bicycle_steering_controller: default_value: 0.0, description: "Traction wheel radius.", read_only: false, - validation: { - gt<>: [0.0] - } + # TODO(anyone): activate validation if front/rear wheel radius is removed + # validation: { + # gt<>: [0.0] + # } } front_wheel_radius: diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 460ca6c38f..a7b6d93ebf 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -82,17 +82,80 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( { params_ = param_listener_->get_params(); + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED + if (!params_.front_steering) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_steering', set 'traction_joint_names' or 'steering_joint_names' " + "instead"); + } + if (params_.front_wheels_names.size() > 0) { - fprintf(stderr, "DEPRECATED parameter 'front_wheels_names'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheels_names', set 'traction_joint_names' or " + "'steering_joint_names' instead"); + if (params_.front_steering) + { + params_.steering_joints_names = params_.front_wheels_names; + } + else + { + params_.traction_joints_names = params_.front_wheels_names; + } } if (params_.rear_wheels_names.size() > 0) { - fprintf(stderr, "DEPRECATED parameter 'rear_wheels_names'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheels_names', set 'traction_joint_names' or " + "'steering_joint_names' instead"); + if (params_.front_steering) + { + params_.traction_joints_names = params_.rear_wheels_names; + } + else + { + params_.steering_joints_names = params_.rear_wheels_names; + } + } + + if (params_.front_wheels_state_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheels_state_names', set 'traction_joints_state_names' or " + "'steering_joints_state_names' instead"); + if (params_.front_steering) + { + params_.steering_joints_state_names = params_.front_wheels_state_names; + } + else + { + params_.traction_joints_state_names = params_.front_wheels_state_names; + } + } + + if (params_.rear_wheels_state_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheels_state_names', set 'traction_joints_state_names' or " + "'steering_joints_state_names' instead"); + if (params_.front_steering) + { + params_.traction_joints_state_names = params_.rear_wheels_state_names; + } + else + { + params_.steering_joints_state_names = params_.rear_wheels_state_names; + } } + // END OF DEPRECATED odometry_.set_velocity_rolling_window_size( static_cast(params_.velocity_rolling_window_size)); diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 452e5c000a..68fc66890c 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -2,13 +2,13 @@ steering_controllers_library: reference_timeout: { type: double, default_value: 1.0, - description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } front_steering: { type: bool, read_only: true, - default_value: false, - description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" + default_value: true, + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names' instead" } front_wheels_names: { @@ -28,22 +28,26 @@ steering_controllers_library: traction_joints_names: { type: string_array, description: "Names of wheel joints.", + default_value: [], read_only: true, validation: { size_lt<>: [5], unique<>: null, - not_empty<>: null, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # not_empty<>: null, } } steering_joints_names: { type: string_array, description: "Names of steering joints.", + default_value: [], read_only: true, validation: { size_lt<>: [5], unique<>: null, - not_empty<>: null, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # not_empty<>: null, } } @@ -58,6 +62,17 @@ steering_controllers_library: } } + rear_wheels_state_names: { + type: string_array, + description: "DEPRECATED: Use 'traction_joints_state_names' or 'steering_joints_state_names'", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + steering_joints_state_names: { type: string_array, description: "(Optional) Names of steering joints to read states from. If not set joint names from 'steering_joints_names' will be used.", @@ -69,6 +84,17 @@ steering_controllers_library: } } + front_wheels_state_names: { + type: string_array, + description: "DEPRECATED: Use 'traction_joints_state_names' or 'steering_joints_state_names'", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + open_loop: { type: bool, default_value: false, diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 858af82f46..9ba2e2e8ac 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -32,14 +32,18 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome if (tricycle_params_.front_wheel_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead"); + tricycle_params_.traction_wheels_radius = tricycle_params_.front_wheel_radius; } if (tricycle_params_.rear_wheel_radius > 0.0) { - fprintf(stderr, "DEPRECATED parameter 'rear_wheel_radius'\n"); - return controller_interface::CallbackReturn::ERROR; + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead"); + tricycle_params_.traction_wheels_radius = tricycle_params_.rear_wheel_radius; } const double traction_wheels_radius = tricycle_params_.traction_wheels_radius; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index 4593389b39..dd9fb44f8c 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -27,9 +27,10 @@ tricycle_steering_controller: default_value: 0.0, description: "Traction wheels radius.", read_only: false, - validation: { - gt<>: [0.0] - } + # TODO(anyone): activate validation if front/rear wheel radius is removed + # validation: { + # gt<>: [0.0] + # } } front_wheel_radius: From 44252a1d69c7de87926f88b173cfe12c3d43dd42 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:11:15 +0000 Subject: [PATCH 04/10] Keep old tricycle parameters --- .../src/tricycle_steering_controller.cpp | 8 ++++---- .../src/tricycle_steering_controller.yaml | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 9ba2e2e8ac..e830b9ffad 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -30,20 +30,20 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { tricycle_params_ = tricycle_param_listener_->get_params(); - if (tricycle_params_.front_wheel_radius > 0.0) + if (tricycle_params_.front_wheels_radius > 0.0) { RCLCPP_WARN( get_node()->get_logger(), "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead"); - tricycle_params_.traction_wheels_radius = tricycle_params_.front_wheel_radius; + tricycle_params_.traction_wheels_radius = tricycle_params_.front_wheels_radius; } - if (tricycle_params_.rear_wheel_radius > 0.0) + if (tricycle_params_.rear_wheels_radius > 0.0) { RCLCPP_WARN( get_node()->get_logger(), "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead"); - tricycle_params_.traction_wheels_radius = tricycle_params_.rear_wheel_radius; + tricycle_params_.traction_wheels_radius = tricycle_params_.rear_wheels_radius; } const double traction_wheels_radius = tricycle_params_.traction_wheels_radius; diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.yaml b/tricycle_steering_controller/src/tricycle_steering_controller.yaml index dd9fb44f8c..1f2ed4dce2 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -33,7 +33,7 @@ tricycle_steering_controller: # } } - front_wheel_radius: + front_wheels_radius: { type: double, default_value: 0.0, @@ -41,7 +41,7 @@ tricycle_steering_controller: read_only: false, } - rear_wheel_radius: + rear_wheels_radius: { type: double, default_value: 0.0, From 622e829029244209c6d44f8f5093be0da9705c78 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:14:55 +0000 Subject: [PATCH 05/10] Reformat yaml to have less diff --- .../src/steering_controllers_library.yaml | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/steering_controllers_library/src/steering_controllers_library.yaml b/steering_controllers_library/src/steering_controllers_library.yaml index 68fc66890c..cfb84eaa96 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -4,6 +4,7 @@ steering_controllers_library: default_value: 1.0, description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } + front_steering: { type: bool, read_only: true, @@ -11,23 +12,20 @@ steering_controllers_library: description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names' instead" } - front_wheels_names: { - type: string_array, - read_only: true, - default_value: [], - description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" - } - rear_wheels_names: { type: string_array, + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'", read_only: true, default_value: [], - description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'" + validation: { + size_lt<>: [5], + unique<>: null, + } } traction_joints_names: { type: string_array, - description: "Names of wheel joints.", + description: "Names of traction wheel joints.", default_value: [], read_only: true, validation: { @@ -38,6 +36,17 @@ steering_controllers_library: } } + front_wheels_names: { + type: string_array, + description: "DEPRECATED: Use 'traction_joint_names' or 'steering_joint_names'", + read_only: true, + default_value: [], + validation: { + size_lt<>: [5], + unique<>: null, + } + } + steering_joints_names: { type: string_array, description: "Names of steering joints.", From ec893549a36e662f9672852d94d081363c7b01a8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:40:18 +0000 Subject: [PATCH 06/10] Add parameter for traction/steering wheel track --- .../src/ackermann_steering_controller.cpp | 38 +++++++++-- .../src/ackermann_steering_controller.yaml | 27 +++++--- doc/release_notes.rst | 1 + .../steering_odometry.hpp | 18 ++++-- .../src/steering_odometry.cpp | 63 +++++++++++-------- 5 files changed, 104 insertions(+), 43 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index a614723654..86927c5424 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "ackermann_steering_controller/ackermann_steering_controller.hpp" namespace ackermann_steering_controller @@ -31,6 +33,8 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { ackermann_params_ = ackermann_param_listener_->get_params(); + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED if (ackermann_params_.front_wheels_radius > 0.0) { RCLCPP_WARN( @@ -51,23 +55,47 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { RCLCPP_WARN( get_node()->get_logger(), - "DEPRECATED parameter 'front_wheel_track', set 'traction_wheel_track' instead"); - ackermann_params_.traction_wheel_track = ackermann_params_.front_wheel_track; + "DEPRECATED parameter 'front_wheel_track', set 'traction_wheel_track' or " + "'steering_wheel_track' instead"); + if (params_.front_steering) + { + ackermann_params_.steering_wheel_track = ackermann_params_.front_wheel_track; + } + else + { + ackermann_params_.traction_wheel_track = ackermann_params_.front_wheel_track; + } } if (ackermann_params_.rear_wheel_track > 0.0) { RCLCPP_WARN( get_node()->get_logger(), - "DEPRECATED parameter 'rear_wheel_track', set 'traction_wheel_track' instead"); - ackermann_params_.traction_wheel_track = ackermann_params_.rear_wheel_track; + "DEPRECATED parameter 'rear_wheel_track', set 'traction_wheel_track' or " + "'steering_wheel_track' instead"); + if (params_.front_steering) + { + ackermann_params_.traction_wheel_track = ackermann_params_.rear_wheel_track; + } + else + { + ackermann_params_.steering_wheel_track = ackermann_params_.rear_wheel_track; + } + } + // END OF DEPRECATED + + if (ackermann_params_.steering_wheel_track <= std::numeric_limits::epsilon()) + { + ackermann_params_.steering_wheel_track = ackermann_params_.traction_wheel_track; } const double traction_wheels_radius = ackermann_params_.traction_wheels_radius; const double traction_wheel_track = ackermann_params_.traction_wheel_track; + const double steering_wheel_track = ackermann_params_.steering_wheel_track; const double wheelbase = ackermann_params_.wheelbase; - odometry_.set_wheel_params(traction_wheels_radius, wheelbase, traction_wheel_track); + odometry_.set_wheel_params( + traction_wheels_radius, wheelbase, steering_wheel_track, traction_wheel_track); odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index dcd0d8b3e5..11d835e2b3 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,4 +1,21 @@ ackermann_steering_controller: + + front_wheel_track: + { + type: double, + default_value: 0.0, + description: "DEPRECATED: use 'traction_wheel_track' or 'steering_wheel_track'", + read_only: false, + } + + steering_wheel_track: + { + type: double, + default_value: 0.0, + description: "(Optional) Steering wheel track length. If not set, 'traction_wheel_track' will be used.", + read_only: false, + } + traction_wheel_track: { type: double, @@ -10,18 +27,12 @@ ackermann_steering_controller: # gt<>: [0.0] # } } - front_wheel_track: - { - type: double, - default_value: 0.0, - description: "DEPRECATED: use 'traction_wheel_track'", - read_only: false, - } + rear_wheel_track: { type: double, default_value: 0.0, - description: "DEPRECATED: use 'traction_wheel_track'", + description: "DEPRECATED: use 'traction_wheel_track' or 'steering_wheel_track'", read_only: false, } diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 74cd0402f1..35cb71f50c 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -67,6 +67,7 @@ steering_controllers_library * A fix for Ackermann steering odometry was added (`#921 `_). * Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). * New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). +* Ackermann kinematics now supports different wheel tracks for traction and steering axle (`#1166 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index ddf9fcdec8..880d3120fc 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -176,10 +176,17 @@ class SteeringOdometry double get_angular() const { return angular_; } /** - * \brief Sets the wheel parameters: radius, separation and wheelbase + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track */ void set_wheel_params( - const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); + const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0); + + /** + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track for steering and traction + */ + void set_wheel_params( + const double wheel_radius, const double wheel_base, const double wheel_track_steering, + const double wheel_track_traction); /** * \brief Velocity rolling window size setter @@ -273,9 +280,10 @@ class SteeringOdometry double angular_; // [rad/s] /// Kinematic parameters - double wheel_track_; // [m] - double wheelbase_; // [m] - double wheel_radius_; // [m] + double wheel_track_traction_; // [m] + double wheel_track_steering_; // [m] + double wheel_base_; // [m] + double wheel_radius_; // [m] /// Configuration type used for the forward kinematics int config_type_ = -1; diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b2bf100255..4f7c844cd8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -31,8 +31,9 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) heading_(0.0), linear_(0.0), angular_(0.0), - wheel_track_(0.0), - wheelbase_(0.0), + wheel_track_traction_(0.0), + wheel_track_steering_(0.0), + wheel_base_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), traction_right_wheel_old_pos_(0.0), @@ -123,7 +124,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -132,7 +133,7 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos) { - double turning_radius = wheelbase_ / std::tan(steer_pos); + double turning_radius = wheel_base_ / std::tan(steer_pos); const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; @@ -142,8 +143,10 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( } // overdetermined, we take the average - const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); - const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); + const double vel_r = + vel_wheel_r * turning_radius / (turning_radius + wheel_track_traction_ * 0.5); + const double vel_l = + vel_wheel_l * turning_radius / (turning_radius - wheel_track_traction_ * 0.5); return (vel_r + vel_l) * 0.5; } @@ -155,7 +158,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -166,16 +169,16 @@ bool SteeringOdometry::update_from_velocity( { // overdetermined, we take the average const double right_steer_pos_est = std::atan( - wheelbase_ * std::tan(right_steer_pos) / - (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + wheel_base_ * std::tan(right_steer_pos) / + (wheel_base_ - wheel_track_steering_ / 2 * std::tan(right_steer_pos))); const double left_steer_pos_est = std::atan( - wheelbase_ * std::tan(left_steer_pos) / - (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + wheel_base_ * std::tan(left_steer_pos) / + (wheel_base_ + wheel_track_steering_ / 2 * std::tan(left_steer_pos))); steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = steer_pos_ * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -190,11 +193,21 @@ void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz integrate_fk(v_bx, omega_bz, dt); } -void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) { wheel_radius_ = wheel_radius; - wheelbase_ = wheelbase; - wheel_track_ = wheel_track; + wheel_base_ = wheel_base; + wheel_track_traction_ = wheel_track; + wheel_track_steering_ = wheel_track; +} + +void SteeringOdometry::set_wheel_params( + double wheel_radius, double wheel_base, double wheel_track_steering, double wheel_track_traction) +{ + wheel_radius_ = wheel_radius; + wheel_base_ = wheel_base; + wheel_track_traction_ = wheel_track_traction; + wheel_track_steering_ = wheel_track_steering; } void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) @@ -212,7 +225,7 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { // phi can be nan if both v_bx and omega_bz are zero - const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + const auto phi = std::atan(omega_bz * wheel_base_ / v_bx); return std::isfinite(phi) ? phi : 0.0; } @@ -286,9 +299,9 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - const double turning_radius = wheelbase_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } // simple steering @@ -308,14 +321,14 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - const double turning_radius = wheelbase_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - const double numerator = 2 * wheelbase_ * std::sin(phi); - const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); - const double denominator_second_member = wheel_track_ * std::sin(phi); + const double numerator = 2 * wheel_base_ * std::sin(phi); + const double denominator_first_member = 2 * wheel_base_ * std::cos(phi); + const double denominator_second_member = wheel_track_steering_ * std::sin(phi); const double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); From 9bc443e6b92e38379c9862c22bdf16bc26918805 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:52:13 +0000 Subject: [PATCH 07/10] Add release and migration notes --- .../src/ackermann_steering_controller.yaml | 16 ++++++++-------- doc/migration.rst | 9 +++++++++ doc/release_notes.rst | 1 + 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 11d835e2b3..7bfe623dea 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -16,24 +16,24 @@ ackermann_steering_controller: read_only: false, } - traction_wheel_track: + rear_wheel_track: { type: double, default_value: 0.0, - description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + description: "DEPRECATED: use 'traction_wheel_track' or 'steering_wheel_track'", read_only: false, - # TODO(anyone): activate validation if front/rear wheel track is removed - # validation: { - # gt<>: [0.0] - # } } - rear_wheel_track: + traction_wheel_track: { type: double, default_value: 0.0, - description: "DEPRECATED: use 'traction_wheel_track' or 'steering_wheel_track'", + description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, + # TODO(anyone): activate validation if front/rear wheel track is removed + # validation: { + # gt<>: [0.0] + # } } wheelbase: diff --git a/doc/migration.rst b/doc/migration.rst index 4f603880bb..f02e2fccae 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -20,3 +20,12 @@ joint_trajectory_controller * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. + +steering_controllers_library +******************************** +* ``front_steering`` parameter was removed (`#1166 `_). Now every kinematics type (bicycle, tricycle, Ackermann) has dedicated parameters for steering or traction wheels instead of front/rear wheels. + + * *General*: Remove ``front_steering`` and use ``traction_joints_names``/``steering_joints_names`` instead of ``rear_wheels_names``/``front_wheels_names``, respectively. + * *bicycle_steering_controller*: Set ``traction_wheel_radius`` instead of ``front_wheel_radius``, ``rear_wheel_radius``. + * *tricycle_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``. + * *ackermann_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``, and ``traction_wheel_track`` (and optionally ``steering_wheel_track``, if it differs) instead of ``rear_wheel_track``, ``front_wheel_track``. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 35cb71f50c..203e490531 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -68,6 +68,7 @@ steering_controllers_library * Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). * New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). * Ackermann kinematics now supports different wheel tracks for traction and steering axle (`#1166 `_). +* ``front_steering`` parameter was removed, see migration notes (`#1166 `_). tricycle_controller ************************ From 7584e3a9d9b346680875caf24c26983a32226ace Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 15:56:33 +0000 Subject: [PATCH 08/10] Update docs --- steering_controllers_library/doc/userdoc.rst | 31 ++++++-------------- 1 file changed, 9 insertions(+), 22 deletions(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 3a941f10c5..c5105868f7 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -36,15 +36,14 @@ The command for the wheels are calculated using ``odometry`` library where based Currently implemented kinematics -------------------------------------------------------------- -* :ref:`Bicycle ` - with one steering and one drive joints; -* :ref:`Tricylce ` - with one steering and two drive joints; -* :ref:`Ackermann ` - with two steering and two drive joints. +* Bicycle - with one steering and one drive joints; +* Tricycle - with one steering and two drive joints; +* Ackermann - with two steering and two drive joints. .. toctree:: - :hidden: Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> - Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Tricycle <../../tricycle_steering_controller/doc/userdoc.rst> Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> Description of controller's interfaces @@ -63,15 +62,8 @@ representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, -If parameter ``front_steering == true`` - -- ``/position`` double, in rad -- ``/velocity`` double, in m/s - -If parameter ``front_steering == false`` - -- ``/velocity`` double, in m/s -- ``/position`` double, in rad +- ``/position`` double, in rad +- ``/velocity`` double, in m/s State interfaces ,,,,,,,,,,,,,,,,, @@ -81,15 +73,10 @@ Depending on the ``position_feedback``, different feedback types are expected * ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` * ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` -If parameter ``front_steering == true`` - -- ``/position`` double, in rad -- ``/`` double, in m or m/s - -If parameter ``front_steering == false`` +With the following state interfaces: -- ``/`` double, in m or m/s -- ``/position`` double, in rad +- ``/position`` double, in rad +- ``/`` double, in m or m/s Subscribers ,,,,,,,,,,,, From 69eb9eae738d3ae867cc74806f89f1fa9022dfb4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 3 Jan 2025 16:06:47 +0000 Subject: [PATCH 09/10] Update format of docs --- steering_controllers_library/doc/userdoc.rst | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index c5105868f7..487cf94bb9 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -38,9 +38,10 @@ Currently implemented kinematics * Bicycle - with one steering and one drive joints; * Tricycle - with one steering and two drive joints; -* Ackermann - with two steering and two drive joints. +* Ackermann - with two steering and two drive joints. .. toctree:: + :titlesonly: Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> Tricycle <../../tricycle_steering_controller/doc/userdoc.rst> From 0db41b5889e0ff33dd91a5e55b4472213d3a0c5f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 3 Jan 2025 20:31:13 +0100 Subject: [PATCH 10/10] Apply suggestions from code review Co-authored-by: Reza Kermani --- .../src/bicycle_steering_controller.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/bicycle_steering_controller/src/bicycle_steering_controller.cpp b/bicycle_steering_controller/src/bicycle_steering_controller.cpp index b8bd2679c6..eb10e5ad00 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -31,6 +31,8 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet { bicycle_params_ = bicycle_param_listener_->get_params(); + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED if (bicycle_params_.front_wheel_radius > 0.0) { RCLCPP_WARN( @@ -46,6 +48,8 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheel_radius' instead"); bicycle_params_.traction_wheel_radius = bicycle_params_.rear_wheel_radius; } + // END OF DEPRECATED + const double wheelbase = bicycle_params_.wheelbase; const double traction_wheel_radius = bicycle_params_.traction_wheel_radius;