diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index 77ba55812a..86927c5424 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include "ackermann_steering_controller/ackermann_steering_controller.hpp" namespace ackermann_steering_controller @@ -31,21 +33,69 @@ controller_interface::CallbackReturn AckermannSteeringController::configure_odom { ackermann_params_ = ackermann_param_listener_->get_params(); - const double front_wheels_radius = ackermann_params_.front_wheels_radius; - const double rear_wheels_radius = ackermann_params_.rear_wheels_radius; - const double front_wheel_track = ackermann_params_.front_wheel_track; - const double rear_wheel_track = ackermann_params_.rear_wheel_track; - const double wheelbase = ackermann_params_.wheelbase; + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED + if (ackermann_params_.front_wheels_radius > 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead"); + ackermann_params_.traction_wheels_radius = ackermann_params_.front_wheels_radius; + } - if (params_.front_steering) + if (ackermann_params_.rear_wheels_radius > 0.0) { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead"); + ackermann_params_.traction_wheels_radius = ackermann_params_.rear_wheels_radius; } - else + + if (ackermann_params_.front_wheel_track > 0.0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_track', set 'traction_wheel_track' or " + "'steering_wheel_track' instead"); + if (params_.front_steering) + { + ackermann_params_.steering_wheel_track = ackermann_params_.front_wheel_track; + } + else + { + ackermann_params_.traction_wheel_track = ackermann_params_.front_wheel_track; + } + } + + if (ackermann_params_.rear_wheel_track > 0.0) { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_track', set 'traction_wheel_track' or " + "'steering_wheel_track' instead"); + if (params_.front_steering) + { + ackermann_params_.traction_wheel_track = ackermann_params_.rear_wheel_track; + } + else + { + ackermann_params_.steering_wheel_track = ackermann_params_.rear_wheel_track; + } } + // END OF DEPRECATED + + if (ackermann_params_.steering_wheel_track <= std::numeric_limits::epsilon()) + { + ackermann_params_.steering_wheel_track = ackermann_params_.traction_wheel_track; + } + + const double traction_wheels_radius = ackermann_params_.traction_wheels_radius; + const double traction_wheel_track = ackermann_params_.traction_wheel_track; + const double steering_wheel_track = ackermann_params_.steering_wheel_track; + const double wheelbase = ackermann_params_.wheelbase; + odometry_.set_wheel_params( + traction_wheels_radius, wheelbase, steering_wheel_track, traction_wheel_track); odometry_.set_odometry_type(steering_odometry::ACKERMANN_CONFIG); set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.yaml b/ackermann_steering_controller/src/ackermann_steering_controller.yaml index 1ec0b41c9f..7bfe623dea 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,24 +1,39 @@ ackermann_steering_controller: + front_wheel_track: { type: double, default_value: 0.0, - description: "Front wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + description: "DEPRECATED: use 'traction_wheel_track' or 'steering_wheel_track'", + read_only: false, + } + + steering_wheel_track: + { + type: double, + default_value: 0.0, + description: "(Optional) Steering wheel track length. If not set, 'traction_wheel_track' will be used.", read_only: false, - validation: { - gt<>: [0.0] - } } 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' or 'steering_wheel_track'", read_only: false, - validation: { - gt<>: [0.0] - } + } + + traction_wheel_track: + { + type: double, + default_value: 0.0, + description: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + # TODO(anyone): activate validation if front/rear wheel track is removed + # validation: { + # gt<>: [0.0] + # } } wheelbase: @@ -32,24 +47,30 @@ ackermann_steering_controller: } } + traction_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Traction wheels radius.", + read_only: false, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # validation: { + # gt<>: [0.0] + # } + } + front_wheels_radius: { type: double, default_value: 0.0, - description: "Front wheels radius.", + description: "DEPRECATED: use 'traction_wheels_radius'", read_only: false, - validation: { - gt<>: [0.0] - } } 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 15eb39f1a4..9c80c7ef5d 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp @@ -31,18 +31,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) @@ -55,32 +52,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 c1cb1d4f16..a24d032c29 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp @@ -152,25 +152,25 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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()); @@ -180,25 +180,25 @@ class AckermannSteeringControllerFixture : public ::testing::Test 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()); @@ -277,29 +277,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 7a0e4d430e..1b950d39ce 100644 --- a/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp +++ b/ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp @@ -31,20 +31,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) @@ -57,32 +54,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..eb10e5ad00 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -31,19 +31,29 @@ controller_interface::CallbackReturn BicycleSteeringController::configure_odomet { bicycle_params_ = bicycle_param_listener_->get_params(); - const double wheelbase = bicycle_params_.wheelbase; - const double front_wheel_radius = bicycle_params_.front_wheel_radius; - const double rear_wheel_radius = bicycle_params_.rear_wheel_radius; - - if (params_.front_steering) + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED + if (bicycle_params_.front_wheel_radius > 0.0) { - odometry_.set_wheel_params(rear_wheel_radius, wheelbase); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheel_radius' instead"); + bicycle_params_.traction_wheel_radius = bicycle_params_.front_wheel_radius; } - else + + if (bicycle_params_.rear_wheel_radius > 0.0) { - odometry_.set_wheel_params(front_wheel_radius, wheelbase); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheel_radius' instead"); + bicycle_params_.traction_wheel_radius = bicycle_params_.rear_wheel_radius; } + // END OF DEPRECATED + + 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..4ce224cca8 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -10,24 +10,30 @@ 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] - } + # TODO(anyone): activate validation if front/rear wheel radius is removed + # 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 9c41f5fd47..23103f7974 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp @@ -31,16 +31,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) @@ -52,19 +50,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 dcec0f1a3d..4cc5f72d4f 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -150,13 +150,14 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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_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; @@ -165,13 +166,14 @@ class BicycleSteeringControllerFixture : public ::testing::Test 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_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)); @@ -253,19 +255,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 500bb36c1c..a469435d24 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp @@ -31,18 +31,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) @@ -55,20 +53,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/doc/migration.rst b/doc/migration.rst index fb7c314f6f..a63940e47a 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -21,3 +21,12 @@ joint_trajectory_controller * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. * Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 `_). + +steering_controllers_library +******************************** +* ``front_steering`` parameter was removed (`#1166 `_). Now every kinematics type (bicycle, tricycle, Ackermann) has dedicated parameters for steering or traction wheels instead of front/rear wheels. + + * *General*: Remove ``front_steering`` and use ``traction_joints_names``/``steering_joints_names`` instead of ``rear_wheels_names``/``front_wheels_names``, respectively. + * *bicycle_steering_controller*: Set ``traction_wheel_radius`` instead of ``front_wheel_radius``, ``rear_wheel_radius``. + * *tricycle_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``. + * *ackermann_steering_controller*: Set ``traction_wheels_radius`` instead of ``front_wheels_radius``, ``rear_wheels_radius``, and ``traction_wheel_track`` (and optionally ``steering_wheel_track``, if it differs) instead of ``rear_wheel_track``, ``front_wheel_track``. diff --git a/doc/release_notes.rst b/doc/release_notes.rst index eaabcc1e91..d407917374 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -70,6 +70,8 @@ steering_controllers_library * A fix for Ackermann steering odometry was added (`#921 `_). * Do not reset the steering wheels to ``0.0`` on timeout (`#1289 `_). * New parameter ``reduce_wheel_speed_until_steering_reached`` was added. If set to true, then the wheel speed(s) is reduced until the steering angle has been reached. Only considered if ``open_loop = false`` (`#1314 `_). +* Ackermann kinematics now supports different wheel tracks for traction and steering axle (`#1166 `_). +* ``front_steering`` parameter was removed, see migration notes (`#1166 `_). tricycle_controller ************************ diff --git a/steering_controllers_library/doc/userdoc.rst b/steering_controllers_library/doc/userdoc.rst index 8889824d62..487cf94bb9 100644 --- a/steering_controllers_library/doc/userdoc.rst +++ b/steering_controllers_library/doc/userdoc.rst @@ -36,15 +36,15 @@ The command for the wheels are calculated using ``odometry`` library where based Currently implemented kinematics -------------------------------------------------------------- -* :ref:`Bicycle ` - with one steering and one drive joints; -* :ref:`Tricylce ` - with one steering and two drive joints; -* :ref:`Ackermann ` - with two steering and two drive joints. +* Bicycle - with one steering and one drive joints; +* Tricycle - with one steering and two drive joints; +* Ackermann - with two steering and two drive joints. .. toctree:: - :hidden: + :titlesonly: Bicycle <../../bicycle_steering_controller/doc/userdoc.rst> - Tricylce <../../tricycle_steering_controller/doc/userdoc.rst> + Tricycle <../../tricycle_steering_controller/doc/userdoc.rst> Ackermann <../../ackermann_steering_controller/doc/userdoc.rst> Description of controller's interfaces @@ -63,15 +63,8 @@ representing the body twist. Command interfaces ,,,,,,,,,,,,,,,,,,, -If parameter ``front_steering == true`` - -- ``/position`` double, in rad -- ``/velocity`` double, in m/s - -If parameter ``front_steering == false`` - -- ``/velocity`` double, in m/s -- ``/position`` double, in rad +- ``/position`` double, in rad +- ``/velocity`` double, in m/s State interfaces ,,,,,,,,,,,,,,,,, @@ -81,15 +74,10 @@ Depending on the ``position_feedback``, different feedback types are expected * ``position_feedback == true`` --> ``TRACTION_FEEDBACK_TYPE = position`` * ``position_feedback == false`` --> ``TRACTION_FEEDBACK_TYPE = velocity`` -If parameter ``front_steering == true`` - -- ``/position`` double, in rad -- ``/`` double, in m or m/s - -If parameter ``front_steering == false`` +With the following state interfaces: -- ``/`` double, in m or m/s -- ``/position`` double, in rad +- ``/position`` double, in rad +- ``/`` double, in m or m/s Subscribers ,,,,,,,,,,,, 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 786132d2b0..4fa0675711 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 @@ -123,8 +123,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/include/steering_controllers_library/steering_odometry.hpp b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp index ddf9fcdec8..7b243e795c 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -145,6 +145,12 @@ class SteeringOdometry */ void set_odometry_type(const unsigned int type); + /** + * \brief Get odometry type + * \return odometry type + */ + unsigned int get_odometry_type() const { return static_cast(config_type_); } + /** * \brief heading getter * \return heading [rad] @@ -176,10 +182,17 @@ class SteeringOdometry double get_angular() const { return angular_; } /** - * \brief Sets the wheel parameters: radius, separation and wheelbase + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track + */ + void set_wheel_params( + const double wheel_radius, const double wheel_base = 0.0, const double wheel_track = 0.0); + + /** + * \brief Sets the wheel parameters: radius, wheel_base, and wheel_track for steering and traction */ void set_wheel_params( - const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0); + const double wheel_radius, const double wheel_base, const double wheel_track_steering, + const double wheel_track_traction); /** * \brief Velocity rolling window size setter @@ -273,9 +286,10 @@ class SteeringOdometry double angular_; // [rad/s] /// Kinematic parameters - double wheel_track_; // [m] - double wheelbase_; // [m] - double wheel_radius_; // [m] + double wheel_track_traction_; // [m] + double wheel_track_steering_; // [m] + double wheel_base_; // [m] + double wheel_radius_; // [m] /// Configuration type used for the forward kinematics int config_type_ = -1; diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 37999b6b36..a0415dfd9f 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -81,27 +81,244 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); + + // TODO(anyone): Remove deprecated parameters + // START OF DEPRECATED + if (!params_.front_steering) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_steering'. Instead, set 'traction_joints_names' or " + "'steering_joints_names'"); + } + + if (params_.front_wheels_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheels_names', set 'traction_joints_names' or " + "'steering_joints_names' instead"); + if (params_.front_steering) + { + params_.steering_joints_names = params_.front_wheels_names; + } + else + { + params_.traction_joints_names = params_.front_wheels_names; + } + } + + if (params_.rear_wheels_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheels_names', set 'traction_joints_names' or " + "'steering_joints_names' instead"); + if (params_.front_steering) + { + params_.traction_joints_names = params_.rear_wheels_names; + } + else + { + params_.steering_joints_names = params_.rear_wheels_names; + } + } + + if (params_.front_wheels_state_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheels_state_names', set 'traction_joints_state_names' or " + "'steering_joints_state_names' instead"); + if (params_.front_steering) + { + params_.steering_joints_state_names = params_.front_wheels_state_names; + } + else + { + params_.traction_joints_state_names = params_.front_wheels_state_names; + } + } + + if (params_.rear_wheels_state_names.size() > 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheels_state_names', set 'traction_joints_state_names' or " + "'steering_joints_state_names' instead"); + if (params_.front_steering) + { + params_.traction_joints_state_names = params_.rear_wheels_state_names; + } + else + { + params_.steering_joints_state_names = params_.rear_wheels_state_names; + } + } + // END OF DEPRECATED + + // Check if the number of traction joints is correct + if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + { + if (params_.traction_joints_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Bicycle configuration requires exactly one traction joint, but %zu were provided", + params_.traction_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + { + if (params_.traction_joints_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires exactly two traction joints, but %zu were provided", + params_.traction_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + { + if (params_.traction_joints_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Ackermann configuration requires exactly two traction joints, but %zu were provided", + params_.traction_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + // Check if the number of steering joints is correct + if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + { + if (params_.steering_joints_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Bicycle configuration requires exactly one steering joint, but %zu were provided", + params_.steering_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + { + if (params_.steering_joints_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires exactly one steering joint, but %zu were provided", + params_.steering_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + { + if (params_.steering_joints_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Ackermann configuration requires exactly two steering joints, but %zu were provided", + params_.steering_joints_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + odometry_.set_velocity_rolling_window_size( static_cast(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; + if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + { + if (params_.traction_joints_state_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Bicycle configuration requires exactly one traction joint, but %zu state interface " + "names were provided", + params_.traction_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + { + if (params_.traction_joints_state_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires exactly two traction joints, but %zu state interface " + "names were provided", + params_.traction_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + { + if (params_.traction_joints_state_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Ackermann configuration requires exactly two traction joints, but %zu state interface " + "names were provided", + params_.traction_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + 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; + if (odometry_.get_odometry_type() == steering_odometry::BICYCLE_CONFIG) + { + if (params_.steering_joints_state_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Bicycle configuration requires exactly one steering joint, but %zu state interface " + "names were provided", + params_.steering_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::TRICYCLE_CONFIG) + { + if (params_.steering_joints_state_names.size() != 1) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Tricycle configuration requires exactly one steering joint, but %zu state interface " + "names were provided", + params_.steering_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + else if (odometry_.get_odometry_type() == steering_odometry::ACKERMANN_CONFIG) + { + if (params_.steering_joints_state_names.size() != 2) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Ackermann configuration requires exactly two steering joints, but %zu state interface " + "names were provided", + params_.steering_joints_state_names.size()); + return controller_interface::CallbackReturn::ERROR; + } + } + 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 @@ -234,34 +451,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; } @@ -276,33 +475,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; @@ -392,30 +575,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c reference_interfaces_[0], reference_interfaces_[1], params_.open_loop, params_.reduce_wheel_speed_until_steering_reached); - 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(timeout ? 0.0 : 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(timeout ? 0.0 : 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(timeout ? 0.0 : 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]); } } @@ -458,14 +624,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 4e7deef698..6ad09822b7 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -2,41 +2,89 @@ steering_controllers_library: reference_timeout: { type: double, default_value: 1.0, - description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behaviour if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", + description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } front_steering: { type: bool, - default_value: true, - description: "Is the steering on the front of the robot?", read_only: true, + default_value: true, + description: "DEPRECATED: Use 'traction_joints_names' or 'steering_joints_names' instead" } rear_wheels_names: { type: string_array, - description: "Names of rear wheel joints.", + description: "DEPRECATED: Use 'traction_joints_names' or 'steering_joints_names' instead", + read_only: true, + default_value: [], + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + traction_joints_names: { + type: string_array, + description: "Names of traction wheel joints. For kinematic configurations with two traction joints, the expected order is: right joint, left joint.", + default_value: [], read_only: true, validation: { size_lt<>: [5], unique<>: null, - not_empty<>: null, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # not_empty<>: null, } } front_wheels_names: { type: string_array, - description: "Names of front wheel joints.", + description: "DEPRECATED: Use 'traction_joints_names' or 'steering_joints_names'' instead", + read_only: true, + default_value: [], + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + steering_joints_names: { + type: string_array, + description: "Names of steering joints. For kinematic configurations with two steering joints, the expected order is: right joint, left joint. The orientation of the steering axes is expected to be in the direction of the z-axis of the vehicle, see REP-103.", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # not_empty<>: null, + } + } + + traction_joints_state_names: { + type: string_array, + 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: { size_lt<>: [5], unique<>: null, - not_empty<>: null, } } rear_wheels_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: "DEPRECATED: Use 'traction_joints_state_names' or 'steering_joints_state_names' instead", + default_value: [], + read_only: true, + validation: { + size_lt<>: [5], + unique<>: null, + } + } + + steering_joints_state_names: { + type: string_array, + description: "(Optional) Names of steering joints to read states from. If not set joint names from 'steering_joints_names' will be used.", default_value: [], read_only: true, validation: { @@ -47,7 +95,7 @@ steering_controllers_library: front_wheels_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: "DEPRECATED: Use 'traction_joints_state_names' or 'steering_joints_state_names' instead", default_value: [], read_only: true, validation: { diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index b8cf847033..5f480ca03d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -33,8 +33,9 @@ SteeringOdometry::SteeringOdometry(size_t velocity_rolling_window_size) heading_(0.0), linear_(0.0), angular_(0.0), - wheel_track_(0.0), - wheelbase_(0.0), + wheel_track_traction_(0.0), + wheel_track_steering_(0.0), + wheel_base_(0.0), wheel_radius_(0.0), traction_wheel_old_pos_(0.0), traction_right_wheel_old_pos_(0.0), @@ -125,7 +126,7 @@ bool SteeringOdometry::update_from_velocity( { steer_pos_ = steer_pos; double linear_velocity = traction_wheel_vel * wheel_radius_; - const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -134,7 +135,7 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( const double right_traction_wheel_vel, const double left_traction_wheel_vel, const double steer_pos) { - double turning_radius = wheelbase_ / std::tan(steer_pos); + double turning_radius = wheel_base_ / std::tan(steer_pos); const double vel_wheel_r = right_traction_wheel_vel * wheel_radius_; const double vel_wheel_l = left_traction_wheel_vel * wheel_radius_; @@ -144,8 +145,10 @@ double SteeringOdometry::get_linear_velocity_double_traction_axle( } // overdetermined, we take the average - const double vel_r = vel_wheel_r * turning_radius / (turning_radius + wheel_track_ * 0.5); - const double vel_l = vel_wheel_l * turning_radius / (turning_radius - wheel_track_ * 0.5); + const double vel_r = + vel_wheel_r * turning_radius / (turning_radius + wheel_track_traction_ * 0.5); + const double vel_l = + vel_wheel_l * turning_radius / (turning_radius - wheel_track_traction_ * 0.5); return (vel_r + vel_l) * 0.5; } @@ -157,7 +160,7 @@ bool SteeringOdometry::update_from_velocity( double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheelbase_; + const double angular_velocity = std::tan(steer_pos_) * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -168,16 +171,16 @@ bool SteeringOdometry::update_from_velocity( { // overdetermined, we take the average const double right_steer_pos_est = std::atan( - wheelbase_ * std::tan(right_steer_pos) / - (wheelbase_ - wheel_track_ / 2 * std::tan(right_steer_pos))); + wheel_base_ * std::tan(right_steer_pos) / + (wheel_base_ - wheel_track_steering_ / 2 * std::tan(right_steer_pos))); const double left_steer_pos_est = std::atan( - wheelbase_ * std::tan(left_steer_pos) / - (wheelbase_ + wheel_track_ / 2 * std::tan(left_steer_pos))); + wheel_base_ * std::tan(left_steer_pos) / + (wheel_base_ + wheel_track_steering_ / 2 * std::tan(left_steer_pos))); steer_pos_ = (right_steer_pos_est + left_steer_pos_est) * 0.5; double linear_velocity = get_linear_velocity_double_traction_axle( right_traction_wheel_vel, left_traction_wheel_vel, steer_pos_); - const double angular_velocity = steer_pos_ * linear_velocity / wheelbase_; + const double angular_velocity = steer_pos_ * linear_velocity / wheel_base_; return update_odometry(linear_velocity, angular_velocity, dt); } @@ -192,11 +195,21 @@ void SteeringOdometry::update_open_loop(const double v_bx, const double omega_bz integrate_fk(v_bx, omega_bz, dt); } -void SteeringOdometry::set_wheel_params(double wheel_radius, double wheelbase, double wheel_track) +void SteeringOdometry::set_wheel_params(double wheel_radius, double wheel_base, double wheel_track) { wheel_radius_ = wheel_radius; - wheelbase_ = wheelbase; - wheel_track_ = wheel_track; + wheel_base_ = wheel_base; + wheel_track_traction_ = wheel_track; + wheel_track_steering_ = wheel_track; +} + +void SteeringOdometry::set_wheel_params( + double wheel_radius, double wheel_base, double wheel_track_steering, double wheel_track_traction) +{ + wheel_radius_ = wheel_radius; + wheel_base_ = wheel_base; + wheel_track_traction_ = wheel_track_traction; + wheel_track_steering_ = wheel_track_steering; } void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_window_size) @@ -214,7 +227,7 @@ void SteeringOdometry::set_odometry_type(const unsigned int type) double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) { // phi can be nan if both v_bx and omega_bz are zero - const auto phi = std::atan(omega_bz * wheelbase_ / v_bx); + const auto phi = std::atan(omega_bz * wheel_base_ / v_bx); return std::isfinite(phi) ? phi : 0.0; } @@ -288,9 +301,9 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - const double turning_radius = wheelbase_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; } // simple steering @@ -310,14 +323,14 @@ std::tuple, std::vector> SteeringOdometry::get_comma } else { - const double turning_radius = wheelbase_ / std::tan(phi_IK); - const double Wr = Ws * (turning_radius + wheel_track_ * 0.5) / turning_radius; - const double Wl = Ws * (turning_radius - wheel_track_ * 0.5) / turning_radius; + const double turning_radius = wheel_base_ / std::tan(phi_IK); + const double Wr = Ws * (turning_radius + wheel_track_traction_ * 0.5) / turning_radius; + const double Wl = Ws * (turning_radius - wheel_track_traction_ * 0.5) / turning_radius; traction_commands = {Wr, Wl}; - const double numerator = 2 * wheelbase_ * std::sin(phi); - const double denominator_first_member = 2 * wheelbase_ * std::cos(phi); - const double denominator_second_member = wheel_track_ * std::sin(phi); + const double numerator = 2 * wheel_base_ * std::sin(phi); + const double denominator_first_member = 2 * wheel_base_ * std::cos(phi); + const double denominator_second_member = wheel_track_steering_ * std::sin(phi); const double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); 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 10a0f00376..26750777c9 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.cpp +++ b/steering_controllers_library/test/test_steering_controllers_library.cpp @@ -36,32 +36,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 38d029d32b..c1a7ec5abb 100644 --- a/steering_controllers_library/test/test_steering_controllers_library.hpp +++ b/steering_controllers_library/test/test_steering_controllers_library.hpp @@ -55,10 +55,8 @@ static constexpr size_t NR_CMD_ITFS = 4; static constexpr size_t NR_REF_ITFS = 2; static constexpr double WHEELBASE_ = 3.24644; -static constexpr double FRONT_WHEEL_TRACK_ = 2.12321; -static constexpr double REAR_WHEEL_TRACK_ = 1.76868; -static constexpr double FRONT_WHEELS_RADIUS_ = 0.45; -static constexpr double REAR_WHEELS_RADIUS_ = 0.45; +static constexpr double WHEELS_TRACK_ = 2.12321; +static constexpr double WHEELS_RADIUS_ = 0.45; namespace { @@ -113,21 +111,21 @@ class TestableSteeringControllersLibrary } // implementing methods which are declared virtual in the steering_controllers_library.hpp - void initialize_implementation_parameter_listener() + void initialize_implementation_parameter_listener() override { param_listener_ = std::make_shared(get_node()); } - controller_interface::CallbackReturn configure_odometry() + controller_interface::CallbackReturn configure_odometry() override { set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); - odometry_.set_wheel_params(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; } - bool update_odometry(const rclcpp::Duration & /*period*/) { return true; } + bool update_odometry(const rclcpp::Duration & /*period*/) override { return true; } }; // We are using template class here for easier reuse of Fixture in specializations of controllers @@ -173,25 +171,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test 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()); @@ -201,25 +199,25 @@ class SteeringControllersLibraryFixture : public ::testing::Test 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()); @@ -298,29 +296,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..e830b9ffad 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -30,20 +30,27 @@ controller_interface::CallbackReturn TricycleSteeringController::configure_odome { tricycle_params_ = tricycle_param_listener_->get_params(); - const double front_wheels_radius = tricycle_params_.front_wheels_radius; - const double rear_wheels_radius = tricycle_params_.rear_wheels_radius; - const double wheel_track = tricycle_params_.wheel_track; - const double wheelbase = tricycle_params_.wheelbase; - - if (params_.front_steering) + if (tricycle_params_.front_wheels_radius > 0.0) { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, wheel_track); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'front_wheel_radius', set 'traction_wheels_radius' instead"); + tricycle_params_.traction_wheels_radius = tricycle_params_.front_wheels_radius; } - else + + if (tricycle_params_.rear_wheels_radius > 0.0) { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); + RCLCPP_WARN( + get_node()->get_logger(), + "DEPRECATED parameter 'rear_wheel_radius', set 'traction_wheels_radius' instead"); + tricycle_params_.traction_wheels_radius = tricycle_params_.rear_wheels_radius; } + 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..1f2ed4dce2 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -21,24 +21,30 @@ tricycle_steering_controller: } } + traction_wheels_radius: + { + type: double, + default_value: 0.0, + description: "Traction wheels radius.", + read_only: false, + # TODO(anyone): activate validation if front/rear wheel radius is removed + # validation: { + # gt<>: [0.0] + # } + } + front_wheels_radius: { type: double, default_value: 0.0, - description: "Front wheels radius.", + description: "DEPRECATED: Use 'traction_wheels_radius'", read_only: false, - validation: { - gt<>: [0.0] - } } 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/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index 93c92c865f..4f9b53e31f 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp @@ -31,16 +31,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_); } @@ -54,25 +52,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 7c6a12938f..065e5d6c52 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -151,19 +151,20 @@ class TricycleSteeringControllerFixture : public ::testing::Test 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; @@ -172,19 +173,20 @@ class TricycleSteeringControllerFixture : public ::testing::Test 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)); @@ -262,27 +264,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 joint_names_{ - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + 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_ = { + 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 preceeding_joint_names_{ - rear_wheels_preceeding_names_[0], rear_wheels_preceeding_names_[1], - front_wheels_preceeding_names_[0]}; + std::vector steers_preceeding_names_ = {"pid_controller/steering_axis_joint"}; + std::vector preceeding_joint_names_ = { + 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 f9b9afb67c..8193cb64ce 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller_preceeding.cpp @@ -30,18 +30,16 @@ TEST_F(TricycleSteeringControllerTest, all_parameters_set_configure_success) ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_THAT( - controller_->params_.rear_wheels_names, - testing::ElementsAreArray(rear_wheels_preceeding_names_)); + controller_->params_.traction_joints_names, + testing::ElementsAreArray(wheels_preceeding_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, - testing::ElementsAreArray(front_wheels_preceeding_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steering_joints_names, + testing::ElementsAreArray(steers_preceeding_names_)); ASSERT_EQ(controller_->params_.open_loop, open_loop_); ASSERT_EQ(controller_->params_.velocity_rolling_window_size, velocity_rolling_window_size_); ASSERT_EQ(controller_->params_.position_feedback, position_feedback_); ASSERT_EQ(controller_->tricycle_params_.wheelbase, wheelbase_); - ASSERT_EQ(controller_->tricycle_params_.front_wheels_radius, front_wheels_radius_); - ASSERT_EQ(controller_->tricycle_params_.rear_wheels_radius, rear_wheels_radius_); + ASSERT_EQ(controller_->tricycle_params_.traction_wheels_radius, traction_wheels_radius_); ASSERT_EQ(controller_->tricycle_params_.wheel_track, wheel_track_); } @@ -55,26 +53,26 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces) ASSERT_EQ(cmd_if_conf.names.size(), joint_command_values_.size()); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_RIGHT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[0] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - preceeding_prefix_ + "/" + rear_wheels_names_[1] + "/" + traction_interface_name_); + preceeding_prefix_ + "/" + traction_joints_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_WHEEL], - preceeding_prefix_ + "/" + front_wheels_names_[0] + "/" + steering_interface_name_); + preceeding_prefix_ + "/" + steering_joints_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); auto state_if_conf = controller_->state_interface_configuration(); ASSERT_EQ(state_if_conf.names.size(), joint_state_values_.size()); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_RIGHT_WHEEL], - controller_->rear_wheels_state_names_[0] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_TRACTION_LEFT_WHEEL], - controller_->rear_wheels_state_names_[1] + "/" + traction_interface_name_); + controller_->traction_joints_state_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( state_if_conf.names[STATE_STEER_AXIS], - controller_->front_wheels_state_names_[0] + "/" + steering_interface_name_); + controller_->steering_joints_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml index cc640e1503..fe12ae5feb 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_params.yaml @@ -2,14 +2,12 @@ test_tricycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_names: [steering_axis_joint] + traction_joints_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_names: [steering_axis_joint] wheelbase: 3.24644 wheel_track: 1.212121 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheels_radius: 0.45 diff --git a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml index 7cb2e4906f..813a961f8f 100644 --- a/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml +++ b/tricycle_steering_controller/test/tricycle_steering_controller_preceeding_params.yaml @@ -1,15 +1,13 @@ test_tricycle_steering_controller: ros__parameters: reference_timeout: 2.0 - front_steering: true open_loop: false velocity_rolling_window_size: 10 position_feedback: false - rear_wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] - front_wheels_names: [pid_controller/steering_axis_joint] - rear_wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] - front_wheels_state_names: [steering_axis_joint] + traction_joints_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + steering_joints_names: [pid_controller/steering_axis_joint] + traction_joints_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steering_joints_state_names: [steering_axis_joint] wheelbase: 3.24644 wheel_track: 1.212121 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheels_radius: 0.45