From c09d17bf00e1b926a7ad74dd5172197e2d207a07 Mon Sep 17 00:00:00 2001 From: Quique Llorente Date: Mon, 6 May 2024 11:47:39 +0200 Subject: [PATCH] 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 | 36 ++++-- .../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 | 22 ++-- .../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 | 122 ++++++------------ .../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 | 24 ++-- .../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, 331 insertions(+), 368 deletions(-) diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..0c9538ecf9 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -30,22 +30,34 @@ void AckermannSteeringController::initialize_implementation_parameter_listener() controller_interface::CallbackReturn AckermannSteeringController::configure_odometry() { ackermann_params_ = ackermann_param_listener_->get_params(); + + if (ackermann_params_.front_wheels_radius > 0.0) { + fprintf(stderr, "DEPRECATED parameter 'front_wheels_radius'\n"); + return controller_interface::CallbackReturn::ERROR; + } + + if (ackermann_params_.rear_wheels_radius > 0.0) { + fprintf(stderr, "DEPRECATED parameter 'rear_wheels_radius'\n"); + return controller_interface::CallbackReturn::ERROR; + } - 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 (params_.front_steering) - { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + if (ackermann_params_.front_wheel_track > 0.0) { + fprintf(stderr, "DEPRECATED parameter 'front_wheel_track'\n"); + return controller_interface::CallbackReturn::ERROR; } - else - { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + + 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 ef5454a16c..af8a72381a 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -32,18 +32,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) @@ -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], - 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 itfsTIME diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49331ae8a2..a83d51ff0c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -165,22 +165,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()); @@ -189,22 +189,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()); @@ -276,29 +276,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 1a16bed838..7ae5522f8c 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -32,20 +32,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) @@ -58,32 +55,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..be0e82217d 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -30,20 +30,20 @@ void BicycleSteeringController::initialize_implementation_parameter_listener() controller_interface::CallbackReturn BicycleSteeringController::configure_odometry() { 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) - { - odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + + if (bicycle_params_.front_wheel_radius > 0.0) { + fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } - else - { - odometry_.set_wheel_params(front_wheel_radius, wheelbase); + + if (bicycle_params_.rear_wheel_radius > 0.0) { + 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 3dcdc0b1db..dea45cbc5b 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -32,16 +32,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) @@ -53,19 +51,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 itfsTIME diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 390447c25f..c6f6de871e 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -162,11 +162,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; @@ -174,11 +176,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)); @@ -251,18 +254,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 bc3a182753..d6a77332bb 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -32,18 +32,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) @@ -56,20 +54,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 a52e34c120..e1ba098517 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -63,13 +63,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 ,,,,,,,,,,,,,,,,, @@ -81,13 +81,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 c11d90dc6f..0d9e8dcb6a 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 @@ -41,7 +41,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 @@ -135,8 +134,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 5880000d17..0572b8bd3f 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -85,26 +85,37 @@ 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 @@ -237,34 +248,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; } @@ -279,33 +272,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; @@ -402,30 +379,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); - 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]); } } @@ -468,14 +428,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 0217567a26..47f1b8cd99 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -38,32 +38,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 itfsTIME diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 05ac17b454..e8f4b262f5 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -57,10 +57,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 { @@ -132,7 +130,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; @@ -186,22 +184,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()); @@ -210,22 +208,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()); @@ -297,29 +295,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..5069354558 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -30,20 +30,22 @@ 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) - { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + + if (tricycle_params_.front_wheel_radius > 0.0) { + fprintf(stderr, "DEPRECATED parameter 'front_wheel_radius'\n"); + return controller_interface::CallbackReturn::ERROR; } - else - { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + + if (tricycle_params_.rear_wheel_radius > 0.0) { + 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 c555de53de..996eab3758 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -32,16 +32,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_); } @@ -55,25 +53,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 1807ab59a8..20c92e3867 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -164,17 +164,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; @@ -182,17 +183,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)); @@ -261,27 +262,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 6f2913aeb8..69f8ea0e5c 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -32,18 +32,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_); } @@ -57,26 +55,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