From 33cc1ccf7fb67c92149b63aff6d20224295aa846 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 718e6f5856..48e51ae7e6 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 itfs diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 363db793d5..2a6bf36845 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 9904f791b6..9d5a56f8e4 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 itfs diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index dcdb7ed169..156d73679f 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 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 e417b9fcd1..dfb0fc858f 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 1aef556212..874fce4fa1 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_, 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]); } } @@ -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 98ab97fdc3..2d9b2c289a 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 itfs diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index b6fe1770c1..49fcf214d0 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 328f5e4d6a..5d965f6ad9 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 b4d58a95c0..1787229fd5 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