diff --git a/ackermann_steering_controller/src/ackermann_steering_controller.cpp b/ackermann_steering_controller/src/ackermann_steering_controller.cpp index d9d95bf8b5..94c7094205 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.cpp +++ b/ackermann_steering_controller/src/ackermann_steering_controller.cpp @@ -31,21 +31,11 @@ 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 traction_wheels_radius = ackermann_params_.traction_wheels_radius; + const double traction_wheel_track = ackermann_params_.traction_wheel_track; const double wheelbase = ackermann_params_.wheelbase; - if (params_.front_steering) - { - odometry_.set_wheel_params(rear_wheels_radius, wheelbase, rear_wheel_track); - } - else - { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, front_wheel_track); - } - + 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..ccfc0257b4 100644 --- a/ackermann_steering_controller/src/ackermann_steering_controller.yaml +++ b/ackermann_steering_controller/src/ackermann_steering_controller.yaml @@ -1,20 +1,9 @@ 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", - 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: "Traction wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", read_only: false, validation: { gt<>: [0.0] @@ -32,22 +21,11 @@ ackermann_steering_controller: } } - front_wheels_radius: - { - type: double, - default_value: 0.0, - description: "Front wheels radius.", - read_only: false, - validation: { - gt<>: [0.0] - } - } - - rear_wheels_radius: + traction_wheels_radius: { type: double, default_value: 0.0, - description: "Rear wheels radius.", + description: "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..ea1d8eb60c 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] + wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steers_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..e4ec6dc2e7 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] + wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + steers_names: [pid_controller/front_right_steering_joint, pid_controller/front_left_steering_joint] + wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steers_state_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 - front_wheel_track: 2.12321 - rear_wheel_track: 1.76868 - front_wheels_radius: 0.45 - rear_wheels_radius: 0.45 + traction_wheel_track: 1.76868 + traction_wheels_radius: 0.45 diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.cpp index ef5454a16c..88b5b6cd8f 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_.wheels_names, testing::ElementsAreArray(wheels_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steers_names, testing::ElementsAreArray(steers_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_); + wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + steers_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + steers_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_->wheels_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_->wheels_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_->steers_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_->steers_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME diff --git a/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp b/ackermann_steering_controller/test/test_ackermann_steering_controller.hpp index 49331ae8a2..326b2d7490 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_, + wheels_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_, + wheels_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_, + steers_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_, + steers_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_, + wheels_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_, + wheels_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_, + steers_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_, + steers_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -276,29 +276,26 @@ 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 wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steers_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]}; + wheels_names_[0], wheels_names_[1], steers_names_[0], steers_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..8b99e6a2e7 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_.wheels_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_.steers_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_ + "/" + wheels_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_ + "/" + wheels_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_ + "/" + steers_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_ + "/" + steers_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_->wheels_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_->wheels_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_->steers_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_->steers_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..46302a1451 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.cpp +++ b/bicycle_steering_controller/src/bicycle_steering_controller.cpp @@ -32,18 +32,9 @@ 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) - { - odometry_.set_wheel_params(rear_wheel_radius, wheelbase); - } - else - { - odometry_.set_wheel_params(front_wheel_radius, 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..2ddaec538f 100644 --- a/bicycle_steering_controller/src/bicycle_steering_controller.yaml +++ b/bicycle_steering_controller/src/bicycle_steering_controller.yaml @@ -10,22 +10,11 @@ bicycle_steering_controller: } } - front_wheel_radius: + traction_wheel_radius: { type: double, default_value: 0.0, - description: "Front wheel radius.", - read_only: false, - validation: { - gt<>: [0.0] - } - } - - rear_wheel_radius: - { - type: double, - default_value: 0.0, - description: "Rear wheel radius.", + description: "Steering wheel radius.", read_only: false, validation: { gt<>: [0.0] diff --git a/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml b/bicycle_steering_controller/test/bicycle_steering_controller_params.yaml index 3a656cc724..d5a52968cc 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] + wheels_names: [rear_wheel_joint] + steers_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..154255b4ae 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] + wheels_names: [pid_controller/rear_wheel_joint] + steers_names: [pid_controller/steering_axis_joint] + wheels_state_names: [rear_wheel_joint] + steers_state_names: [steering_axis_joint] wheelbase: 3.24644 - front_wheel_radius: 0.45 - rear_wheel_radius: 0.45 + traction_wheel_radius: 0.45 diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.cpp index 3dcdc0b1db..385cc25b55 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_.wheels_names, testing::ElementsAreArray(wheels_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steers_names, testing::ElementsAreArray(steers_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,19 @@ 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], wheels_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], steers_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_->wheels_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_->steers_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME diff --git a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp index 390447c25f..2d168386e3 100644 --- a/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp +++ b/bicycle_steering_controller/test/test_bicycle_steering_controller.hpp @@ -162,11 +162,11 @@ 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])); + wheels_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])); + steers_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -174,11 +174,11 @@ 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])); + wheels_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])); + steers_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 +251,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 wheels_names_ = {"rear_wheel_joint"}; + std::vector steers_names_ = {"steering_axis_joint"}; + std::vector joint_names_ = {wheels_names_[0], steers_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..9a8e811cba 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_.wheels_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_.steers_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_ + "/" + wheels_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_ + "/" + steers_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_->wheels_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_->steers_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/include/steering_controllers_library/steering_controllers_library.hpp b/steering_controllers_library/include/steering_controllers_library/steering_controllers_library.hpp index c11d90dc6f..4d952261f3 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 @@ -135,8 +135,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 wheels_state_names_; + std::vector steers_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 0104b011c7..818ac0bd64 100644 --- a/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp +++ b/steering_controllers_library/include/steering_controllers_library/steering_odometry.hpp @@ -36,6 +36,7 @@ namespace steering_odometry const unsigned int BICYCLE_CONFIG = 0; const unsigned int TRICYCLE_CONFIG = 1; const unsigned int ACKERMANN_CONFIG = 2; +const unsigned int SWERVE_CONFIG = 3; /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) diff --git a/steering_controllers_library/src/steering_controllers_library.cpp b/steering_controllers_library/src/steering_controllers_library.cpp index 5880000d17..cd7bfbb9c0 100644 --- a/steering_controllers_library/src/steering_controllers_library.cpp +++ b/steering_controllers_library/src/steering_controllers_library.cpp @@ -89,22 +89,22 @@ controller_interface::CallbackReturn SteeringControllersLibrary::on_configure( configure_odometry(); - if (!params_.rear_wheels_state_names.empty()) + if (!params_.wheels_state_names.empty()) { - rear_wheels_state_names_ = params_.rear_wheels_state_names; + wheels_state_names_ = params_.wheels_state_names; } else { - rear_wheels_state_names_ = params_.rear_wheels_names; + wheels_state_names_ = params_.wheels_names; } - if (!params_.front_wheels_state_names.empty()) + if (!params_.steers_state_names.empty()) { - front_wheels_state_names_ = params_.front_wheels_state_names; + steers_state_names_ = params_.steers_state_names; } else { - front_wheels_state_names_ = params_.front_wheels_names; + steers_state_names_ = params_.steers_names; } // topics QoS @@ -237,34 +237,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_.wheels_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_.wheels_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_.steers_names.size(); i++) + { + command_interfaces_config.names.push_back( + params_.steers_names[i] + "/" + hardware_interface::HW_IF_POSITION); } return command_interfaces_config; } @@ -279,33 +261,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 < wheels_state_names_.size(); i++) { - 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); - } + state_interfaces_config.names.push_back( + wheels_state_names_[i] + "/" + traction_wheels_feedback); } - else - { - 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); - } - 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 < steers_state_names_.size(); i++) + { + state_interfaces_config.names.push_back( + steers_state_names_[i] + "/" + hardware_interface::HW_IF_POSITION); } return state_interfaces_config; @@ -402,30 +368,13 @@ controller_interface::return_type SteeringControllersLibrary::update_and_write_c auto [traction_commands, steering_commands] = odometry_.get_commands(last_linear_velocity_, last_angular_velocity_); - if (params_.front_steering) + for (size_t i = 0; i < params_.wheels_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_.steers_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_.wheels_names.size()].set_value(steering_commands[i]); } } @@ -468,14 +417,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_.wheels_names.size(); + auto number_of_steering_wheels = params_.steers_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..00fd268295 100644 --- a/steering_controllers_library/src/steering_controllers_library.yaml +++ b/steering_controllers_library/src/steering_controllers_library.yaml @@ -5,16 +5,9 @@ steering_controllers_library: 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, - } - - rear_wheels_names: { + wheels_names: { type: string_array, - description: "Names of rear wheel joints.", + description: "Names of wheel joints.", read_only: true, validation: { size_lt<>: [5], @@ -23,9 +16,9 @@ steering_controllers_library: } } - front_wheels_names: { + steers_names: { type: string_array, - description: "Names of front wheel joints.", + description: "Names of steering joints.", read_only: true, validation: { size_lt<>: [5], @@ -34,9 +27,9 @@ steering_controllers_library: } } - rear_wheels_state_names: { + 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: "(Optional) Names of wheel joints to read states from. If not set joint names from 'wheels_names' will be used.", default_value: [], read_only: true, validation: { @@ -45,9 +38,9 @@ steering_controllers_library: } } - front_wheels_state_names: { + steers_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 wheel joints to read states from. If not set joint names from 'steer_names' will be used.", 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 32cdb69454..90e3904e6d 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -258,6 +258,41 @@ std::tuple, std::vector> SteeringOdometry::get_comma } return std::make_tuple(traction_commands, steering_commands); } + else if (config_type_ == SWERVE_CONFIG) + { + std::vector traction_commands; + std::vector steering_commands; + if (fabs(steer_pos_) < 1e-6) + { + traction_commands = {Ws, Ws, Ws, Ws}; + steering_commands = {phi, phi, phi, phi}; + } + else + { + //TODO: this is a simplifierd swerve drive were + // instantenous center is alwys in the middle and + // kingping distance is zero. + double instantaneous_center = wheelbase_ * 0.5; + double turning_radius = instantaneous_center / std::tan(steer_pos_); + + double numerator = 2 * instantaneous_center * std::sin(phi); + double denominator_first_member = 2 * instantaneous_center * std::cos(phi); + double denominator_second_member = wheel_track_ * std::sin(phi); + + double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member); + double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member); + steering_commands = {alpha_r, alpha_l, -alpha_r, -alpha_l}; + + double radius_r = instantaneous_center / std::sin(alpha_r); + double radius_l = instantaneous_center / std::sin(alpha_l); + + double Wr = Ws * (radius_r / turning_radius); + double Wl = Ws * (radius_l / turning_radius); + traction_commands = {Wr, Wl, Wr, Wl}; + + } + return std::make_tuple(traction_commands, steering_commands); + } else { throw std::runtime_error("Config not implemented"); diff --git a/steering_controllers_library/test/steering_controllers_library_params.yaml b/steering_controllers_library/test/steering_controllers_library_params.yaml index 01bfb02302..62f81b7a5b 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -6,8 +6,8 @@ test_steering_controllers_library: 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] + wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steers_names: [front_right_steering_joint, front_left_steering_joint] wheelbase: 3.24644 front_wheel_track: 2.12321 diff --git a/steering_controllers_library/test/test_steering_controllers_library.cpp b/steering_controllers_library/test/test_steering_controllers_library.cpp index 0217567a26..d498e5e498 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_); + wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + wheels_names_[1] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_RIGHT_WHEEL], - front_wheels_names_[0] + "/" + steering_interface_name_); + steers_names_[0] + "/" + steering_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_STEER_LEFT_WHEEL], - front_wheels_names_[1] + "/" + steering_interface_name_); + steers_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_->wheels_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_->wheels_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_->steers_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_->steers_state_names_[1] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfsTIME diff --git a/steering_controllers_library/test/test_steering_controllers_library.hpp b/steering_controllers_library/test/test_steering_controllers_library.hpp index 05ac17b454..03f7b3a196 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_, + wheels_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_, + wheels_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_, + steers_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_, + steers_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_, + wheels_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_, + wheels_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_, + steers_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_, + steers_names_[1], steering_interface_name_, &joint_state_values_[STATE_STEER_LEFT_WHEEL])); state_ifs.emplace_back(state_itfs_.back()); @@ -297,29 +295,22 @@ 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 wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steers_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]}; + wheels_names_[0], wheels_names_[1], steers_names_[0], steers_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/steering_controllers_library/test/test_steering_odometry.cpp b/steering_controllers_library/test/test_steering_odometry.cpp index 3e4adc59fe..69a63f0444 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -109,6 +109,63 @@ TEST(TestSteeringOdometry, ackermann_back_kin_right) EXPECT_LT(cmd1[0], 0); } +TEST(TestSteeringOdometry, swerve_back_kin_linear) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::SWERVE_CONFIG); + odom.update_open_loop(1., 0., 1.); + auto cmd = odom.get_commands(1., 0.); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_EQ(cmd0[0], cmd0[1]); // linear + EXPECT_EQ(cmd0[0], cmd0[2]); // linear + EXPECT_EQ(cmd0[0], cmd0[3]); // linear + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_EQ(cmd1[0], cmd1[1]); // no steering + EXPECT_EQ(cmd1[0], cmd1[2]); // no steering + EXPECT_EQ(cmd1[0], cmd1[3]); // no steering + EXPECT_EQ(cmd1[0], 0); +} + +TEST(TestSteeringOdometry, swerve_back_kin_left) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::SWERVE_CONFIG); + odom.update_from_position(0., 0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., 0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_GT(cmd0[0], cmd0[1]); // front right (outer) > front left (inner) + EXPECT_GT(cmd0[0], 0); + EXPECT_EQ(cmd0[2], cmd0[0]); // rear right == front right + EXPECT_EQ(cmd0[3], cmd0[1]); // rear left == front left + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // front right (outer) < front left (inner) + EXPECT_GT(cmd1[0], 0); + EXPECT_EQ(cmd1[2], -cmd1[0]); // rear right == - front right + EXPECT_EQ(cmd1[3], -cmd1[1]); // rear left == - front left +} + +TEST(TestSteeringOdometry, swerve_back_kin_right) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 2., 1.); + odom.set_odometry_type(steering_odometry::SWERVE_CONFIG); + odom.update_from_position(0., -0.2, 1.); // assume already turn + auto cmd = odom.get_commands(1., -0.1); + auto cmd0 = std::get<0>(cmd); // vel + EXPECT_LT(cmd0[0], cmd0[1]); // front right (inner) < front left (outer) + EXPECT_GT(cmd0[0], 0); + EXPECT_EQ(cmd0[2], cmd0[0]); // rear right == front right + EXPECT_EQ(cmd0[3], cmd0[1]); // rear left == front left + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // front right (outer) < front left (inner) + EXPECT_LT(cmd1[0], 0); + EXPECT_EQ(cmd1[2], -cmd1[0]); // rear right == - front right + EXPECT_EQ(cmd1[3], -cmd1[1]); // rear left == - front left +} + TEST(TestSteeringOdometry, bicycle_odometry) { steering_odometry::SteeringOdometry odom(1); @@ -144,3 +201,17 @@ TEST(TestSteeringOdometry, ackermann_odometry) EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } + +TEST(TestSteeringOdometry, swerve_odometry) +{ + steering_odometry::SteeringOdometry odom(1); + odom.set_wheel_params(1., 1., 1.); + odom.set_odometry_type(steering_odometry::SWERVE_CONFIG); + ASSERT_TRUE(odom.update_from_velocity(1., 1., .1, .1, .1)); + EXPECT_NEAR(odom.get_linear(), 1.0, 1e-3); + EXPECT_NEAR(odom.get_angular(), .1, 1e-3); + EXPECT_NEAR(odom.get_x(), .1, 1e-3); + EXPECT_NEAR(odom.get_heading(), .01, 1e-3); +} + + diff --git a/swerve_drive_controller/CHANGELOG.rst b/swerve_drive_controller/CHANGELOG.rst new file mode 100644 index 0000000000..4c3e93a53b --- /dev/null +++ b/swerve_drive_controller/CHANGELOG.rst @@ -0,0 +1,194 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package swerve_drive_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.23.0 (2024-04-30) +------------------- + +3.22.0 (2024-02-12) +------------------- +* Add test_depend on `hardware_interface_testing` (backport `#1018 `_) (`#1020 `_) +* Add tests for `interface_configuration_type` consistently (backport `#899 `_) (`#1007 `_) +* Contributors: mergify[bot] + +3.21.0 (2024-01-20) +------------------- + +3.20.2 (2024-01-11) +------------------- + +3.20.1 (2024-01-08) +------------------- + +3.20.0 (2024-01-03) +------------------- + +3.19.2 (2023-12-12) +------------------- + +3.19.1 (2023-12-05) +------------------- + +3.19.0 (2023-12-01) +------------------- + +3.18.0 (2023-11-21) +------------------- + +3.17.0 (2023-10-31) +------------------- +* Improve docs (`#785 `_) +* Contributors: Christoph Fröhlich + +3.16.0 (2023-09-20) +------------------- + +3.15.0 (2023-09-11) +------------------- + +3.14.0 (2023-08-16) +------------------- + +3.13.0 (2023-08-04) +------------------- + +3.12.0 (2023-07-18) +------------------- + +3.11.0 (2023-06-24) +------------------- +* Added -Wconversion flag and fix warnings (`#667 `_) +* Let sphinx add parameter description to documentation (`#651 `_) +* Contributors: Christoph Fröhlich, gwalck + +3.10.1 (2023-06-06) +------------------- + +3.10.0 (2023-06-04) +------------------- + +3.9.0 (2023-05-28) +------------------ +* Fix sphinx for steering odometry library/controllers (`#626 `_) +* Steering odometry library and controllers (`#484 `_) +* Contributors: Bence Magyar, Christoph Fröhlich, Tomislav Petković + +3.8.0 (2023-05-14) +------------------ + +3.7.0 (2023-05-02) +------------------ + +3.6.0 (2023-04-29) +------------------ + +3.5.0 (2023-04-14) +------------------ + +3.4.0 (2023-04-02) +------------------ + +3.3.0 (2023-03-07) +------------------ + +3.2.0 (2023-02-10) +------------------ + +3.1.0 (2023-01-26) +------------------ + +3.0.0 (2023-01-19) +------------------ + +2.15.0 (2022-12-06) +------------------- + +2.14.0 (2022-11-18) +------------------- + +2.13.0 (2022-10-05) +------------------- + +2.12.0 (2022-09-01) +------------------- + +2.11.0 (2022-08-04) +------------------- + +2.10.0 (2022-08-01) +------------------- + +2.9.0 (2022-07-14) +------------------ + +2.8.0 (2022-07-09) +------------------ + +2.7.0 (2022-07-03) +------------------ + +2.6.0 (2022-06-18) +------------------ + +2.5.0 (2022-05-13) +------------------ + +2.4.0 (2022-04-29) +------------------ + +2.3.0 (2022-04-21) +------------------ + +2.2.0 (2022-03-25) +------------------ + +2.1.0 (2022-02-23) +------------------ + +2.0.1 (2022-02-01) +------------------ + +2.0.0 (2022-01-28) +------------------ + +1.3.0 (2022-01-11) +------------------ + +1.2.0 (2021-12-29) +------------------ + +1.1.0 (2021-10-25) +------------------ + +1.0.0 (2021-09-29) +------------------ + +0.5.0 (2021-08-30) +------------------ + +0.4.1 (2021-07-08) +------------------ + +0.4.0 (2021-06-28) +------------------ + +0.3.1 (2021-05-23) +------------------ + +0.3.0 (2021-05-21) +------------------ + +0.2.1 (2021-05-03) +------------------ + +0.2.0 (2021-02-06) +------------------ + +0.1.2 (2021-01-07) +------------------ + +0.1.1 (2021-01-06) +------------------ + +0.1.0 (2020-12-23) +------------------ diff --git a/swerve_drive_controller/CMakeLists.txt b/swerve_drive_controller/CMakeLists.txt new file mode 100644 index 0000000000..21942ecd7a --- /dev/null +++ b/swerve_drive_controller/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.16) +project(swerve_drive_controller LANGUAGES CXX) + +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=conversion -Werror=unused-but-set-variable + -Werror=return-type -Werror=shadow -Werror=format) +endif() + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + hardware_interface + generate_parameter_library + pluginlib + rclcpp + rclcpp_lifecycle + realtime_tools + std_srvs + steering_controllers_library +) + +find_package(ament_cmake REQUIRED) +find_package(backward_ros REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() +add_compile_definitions(RCPPUTILS_VERSION_MAJOR=${rcpputils_VERSION_MAJOR}) +add_compile_definitions(RCPPUTILS_VERSION_MINOR=${rcpputils_VERSION_MINOR}) + +generate_parameter_library(swerve_drive_controller_parameters + src/swerve_drive_controller.yaml +) + +add_library( + swerve_drive_controller + SHARED + src/swerve_drive_controller.cpp +) +target_compile_features(swerve_drive_controller PUBLIC cxx_std_17) +target_include_directories(swerve_drive_controller PUBLIC + $ + $) +target_link_libraries(swerve_drive_controller PUBLIC + swerve_drive_controller_parameters) +ament_target_dependencies(swerve_drive_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(swerve_drive_controller PRIVATE "SWERVE_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL") + +pluginlib_export_plugin_description_file( + controller_interface swerve_drive_controller.xml) + +if(BUILD_TESTING) + find_package(ament_cmake_gmock REQUIRED) + find_package(controller_manager REQUIRED) + find_package(hardware_interface REQUIRED) + find_package(ros2_control_test_assets REQUIRED) + + #add_rostest_with_parameters_gmock(test_load_swerve_drive_controller + # test/test_load_swerve_drive_controller.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/swerve_drive_controller_params.yaml + #) + #ament_target_dependencies(test_load_swerve_drive_controller + # controller_manager + # hardware_interface + # ros2_control_test_assets + #) + + add_rostest_with_parameters_gmock(test_swerve_drive_controller + test/test_swerve_drive_controller.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/test/swerve_drive_controller_params.yaml + ) + target_include_directories(test_swerve_drive_controller PRIVATE include) + target_link_libraries(test_swerve_drive_controller swerve_drive_controller) + ament_target_dependencies( + test_swerve_drive_controller + controller_interface + hardware_interface + ) + + #add_rostest_with_parameters_gmock( + # test_swerve_drive_controller_preceeding test/test_swerve_drive_controller_preceeding.cpp + # ${CMAKE_CURRENT_SOURCE_DIR}/test/swerve_drive_controller_preceeding_params.yaml) + #target_include_directories(test_swerve_drive_controller_preceeding PRIVATE include) + #target_link_libraries(test_swerve_drive_controller_preceeding swerve_drive_controller) + #ament_target_dependencies( + # test_swerve_drive_controller_preceeding + # controller_interface + # hardware_interface + #) +endif() + +install( + DIRECTORY include/ + DESTINATION include/swerve_drive_controller +) + +install( + TARGETS swerve_drive_controller swerve_drive_controller_parameters + EXPORT export_swerve_drive_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) + +ament_export_targets(export_swerve_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/swerve_drive_controller/doc/userdoc.rst b/swerve_drive_controller/doc/userdoc.rst new file mode 100644 index 0000000000..852bfdf2bd --- /dev/null +++ b/swerve_drive_controller/doc/userdoc.rst @@ -0,0 +1,24 @@ +:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/swerve_drive_controller/doc/userdoc.rst + +.. _swerve_drive_controller_userdoc: + +swerve_drive_controller +============================= + +This controller implements the kinematics with two axes and four wheels, where the wheels on one axis are fixed (traction/drive) wheels, and the wheels on the other axis are steerable. + +The controller expects to have two commanding joints for traction, one for each fixed wheel and two commanding joints for steering, one for each wheel. + +For more details on controller's execution and interfaces check the :ref:`Steering Controller Library `. + + +Parameters +,,,,,,,,,,, + +This controller uses the `generate_parameter_library `_ to handle its parameters. + +For an exemplary parameterization see the ``test`` folder of the controller's package. + +Additionally to the parameters of the :ref:`Steering Controller Library `, this controller adds the following parameters: + +.. generate_parameter_library_details:: ../src/swerve_drive_controller.yaml diff --git a/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp new file mode 100644 index 0000000000..00a2bb1c1a --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl +// + +#ifndef SWERVE_DRIVE_CONTROLLER__SWERVEN_STEERING_CONTROLLER_HPP_ +#define SWERVE_DRIVE_CONTROLLER__SWERVEN_STEERING_CONTROLLER_HPP_ + +#include + +#include "swerve_drive_controller/visibility_control.h" +#include "swerve_drive_controller_parameters.hpp" +#include "steering_controllers_library/steering_controllers_library.hpp" + +namespace swerve_drive_controller +{ +// name constants for state interfaces +static constexpr size_t STATE_TRACTION_FRONT_RIGHT_WHEEL = 0; +static constexpr size_t STATE_TRACTION_FRONT_LEFT_WHEEL = 1; +static constexpr size_t STATE_TRACTION_REAR_RIGHT_WHEEL = 2; +static constexpr size_t STATE_TRACTION_REAR_LEFT_WHEEL = 3; +static constexpr size_t STATE_STEER_FRONT_RIGHT_WHEEL = 4; +static constexpr size_t STATE_STEER_FRONT_LEFT_WHEEL = 5; +static constexpr size_t STATE_STEER_REAR_RIGHT_WHEEL = 6; +static constexpr size_t STATE_STEER_REAR_LEFT_WHEEL = 7; + +// name constants for command interfaces +static constexpr size_t CMD_TRACTION_FRONT_RIGHT_WHEEL = 0; +static constexpr size_t CMD_TRACTION_FRONT_LEFT_WHEEL = 1; +static constexpr size_t CMD_TRACTION_REAR_RIGHT_WHEEL = 2; +static constexpr size_t CMD_TRACTION_REAR_LEFT_WHEEL = 3; +static constexpr size_t CMD_STEER_FRONT_RIGHT_WHEEL = 4; +static constexpr size_t CMD_STEER_FRONT_LEFT_WHEEL = 5; +static constexpr size_t CMD_STEER_REAR_RIGHT_WHEEL = 6; +static constexpr size_t CMD_STEER_REAR_LEFT_WHEEL = 7; + +static constexpr size_t NR_STATE_ITFS = 8; +static constexpr size_t NR_CMD_ITFS = 8; +static constexpr size_t NR_REF_ITFS = 2; + +class SwerveDriveController : public steering_controllers_library::SteeringControllersLibrary +{ +public: + SwerveDriveController(); + + SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC controller_interface::CallbackReturn + configure_odometry() override; + + SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC bool update_odometry( + const rclcpp::Duration & period) override; + + SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC void + initialize_implementation_parameter_listener() override; + +protected: + std::shared_ptr swerve_param_listener_; + swerve_drive_controller::Params swerve_params_; +}; +} // namespace swerve_drive_controller + +#endif // SWERVE_DRIVE_CONTROLLER__SWERVEN_STEERING_CONTROLLER_HPP_ diff --git a/swerve_drive_controller/include/swerve_drive_controller/visibility_control.h b/swerve_drive_controller/include/swerve_drive_controller/visibility_control.h new file mode 100644 index 0000000000..59ac160471 --- /dev/null +++ b/swerve_drive_controller/include/swerve_drive_controller/visibility_control.h @@ -0,0 +1,52 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SWERVE_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((dllexport)) +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_IMPORT __attribute__((dllimport)) +#else +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_EXPORT __declspec(dllexport) +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_IMPORT __declspec(dllimport) +#endif +#ifdef SWERVE_DRIVE_CONTROLLER__VISIBILITY_BUILDING_DLL +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC \ + SWERVE_DRIVE_CONTROLLER__VISIBILITY_EXPORT +#else +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC \ + SWERVE_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#endif +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE \ + SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#else +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_EXPORT __attribute__((visibility("default"))) +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_IMPORT +#if __GNUC__ >= 4 +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC __attribute__((visibility("default"))) +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_LOCAL __attribute__((visibility("hidden"))) +#else +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_LOCAL +#endif +#define SWERVE_DRIVE_CONTROLLER__VISIBILITY_PUBLIC_TYPE +#endif + +#endif // SWERVE_DRIVE_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/swerve_drive_controller/package.xml b/swerve_drive_controller/package.xml new file mode 100644 index 0000000000..0e4eb92b30 --- /dev/null +++ b/swerve_drive_controller/package.xml @@ -0,0 +1,37 @@ + + + + swerve_drive_controller + 3.23.0 + Steering controller for Swerven kinematics. Rear fixed wheels are powering the vehicle and front wheels are steering it. + Apache License 2.0 + Bence Magyar + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + Dr.-Ing. Denis Štogl + dr. sc. Tomislav Petkovic + + ament_cmake + + generate_parameter_library + + control_msgs + controller_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + std_srvs + steering_controllers_library + + ament_cmake_gmock + controller_manager + hardware_interface_testing + hardware_interface + ros2_control_test_assets + + + ament_cmake + + diff --git a/swerve_drive_controller/src/swerve_drive_controller.cpp b/swerve_drive_controller/src/swerve_drive_controller.cpp new file mode 100644 index 0000000000..5ddecd7f4c --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "swerve_drive_controller/swerve_drive_controller.hpp" + +namespace swerve_drive_controller +{ +SwerveDriveController::SwerveDriveController() +: steering_controllers_library::SteeringControllersLibrary() +{ +} + +void SwerveDriveController::initialize_implementation_parameter_listener() +{ + swerve_param_listener_ = + std::make_shared(get_node()); +} + +controller_interface::CallbackReturn SwerveDriveController::configure_odometry() +{ + swerve_params_ = swerve_param_listener_->get_params(); + + const double wheels_radius = swerve_params_.wheels_radius; + const double wheel_track = swerve_params_.wheel_track; + const double wheelbase = swerve_params_.wheelbase; + + odometry_.set_wheel_params(wheels_radius, wheelbase, wheel_track); + + odometry_.set_odometry_type(steering_odometry::SWERVE_CONFIG); + + set_interface_numbers(NR_STATE_ITFS, NR_CMD_ITFS, NR_REF_ITFS); + + RCLCPP_INFO(get_node()->get_logger(), "swerve odom configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +bool SwerveDriveController::update_odometry(const rclcpp::Duration & period) +{ + if (params_.open_loop) + { + odometry_.update_open_loop(last_linear_velocity_, last_angular_velocity_, period.seconds()); + } + else + { + + //TODO: Take into account REAR wheels and steering joints + const double front_right_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_RIGHT_WHEEL].get_value(); + const double front_left_wheel_value = state_interfaces_[STATE_TRACTION_FRONT_LEFT_WHEEL].get_value(); + const double front_right_steer_position = + state_interfaces_[STATE_STEER_FRONT_RIGHT_WHEEL].get_value(); + const double front_left_steer_position = state_interfaces_[STATE_STEER_FRONT_LEFT_WHEEL].get_value(); + if ( + !std::isnan(front_right_wheel_value) && !std::isnan(front_left_wheel_value) && + !std::isnan(front_right_steer_position) && !std::isnan(front_left_steer_position)) + { + if (params_.position_feedback) + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_position( + front_right_wheel_value, front_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + else + { + // Estimate linear and angular velocity using joint information + odometry_.update_from_velocity( + front_right_wheel_value, front_left_wheel_value, front_right_steer_position, + front_left_steer_position, period.seconds()); + } + } + } + return true; +} +} // namespace swerve_drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + swerve_drive_controller::SwerveDriveController, + controller_interface::ChainableControllerInterface) diff --git a/swerve_drive_controller/src/swerve_drive_controller.yaml b/swerve_drive_controller/src/swerve_drive_controller.yaml new file mode 100644 index 0000000000..83d6d2cca4 --- /dev/null +++ b/swerve_drive_controller/src/swerve_drive_controller.yaml @@ -0,0 +1,24 @@ +swerve_drive_controller: + wheel_track: + { + type: double, + default_value: 0.0, + description: "Wheel track length. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheelbase: + { + type: double, + default_value: 0.0, + description: "Distance between front and rear wheels. For details see: https://en.wikipedia.org/wiki/Wheelbase", + read_only: false, + } + + wheels_radius: + { + type: double, + default_value: 0.0, + description: "Wheels radius.", + read_only: false, + } diff --git a/swerve_drive_controller/swerve_drive_controller.xml b/swerve_drive_controller/swerve_drive_controller.xml new file mode 100644 index 0000000000..10a55b6b31 --- /dev/null +++ b/swerve_drive_controller/swerve_drive_controller.xml @@ -0,0 +1,8 @@ + + + + Steering controller for Swerven (car-like) kinematics with two traction and two steering joints. + + + diff --git a/swerve_drive_controller/test/swerve_drive_controller_params.yaml b/swerve_drive_controller/test/swerve_drive_controller_params.yaml new file mode 100644 index 0000000000..af2501efa7 --- /dev/null +++ b/swerve_drive_controller/test/swerve_drive_controller_params.yaml @@ -0,0 +1,14 @@ +test_swerve_drive_controller: + ros__parameters: + + reference_timeout: 2.0 + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + steers_names: [front_right_steering_joint, front_left_steering_joint, rear_right_steering_joint, rear_left_steering_joint] + wheels_names: [front_right_wheel_joint, front_left_wheel_joint, rear_right_wheel_joint, rear_left_wheel_joint] + + wheelbase: 3.24644 + wheel_track: 2.12321 + wheels_radius: 0.45 diff --git a/swerve_drive_controller/test/swerve_drive_controller_preceeding_params.yaml b/swerve_drive_controller/test/swerve_drive_controller_preceeding_params.yaml new file mode 100644 index 0000000000..e99d36f152 --- /dev/null +++ b/swerve_drive_controller/test/swerve_drive_controller_preceeding_params.yaml @@ -0,0 +1,30 @@ +test_swerve_drive_controller: + ros__parameters: + reference_timeout: 2.0 + open_loop: false + velocity_rolling_window_size: 10 + position_feedback: false + use_stamped_vel: true + wheels_names: + - pid_controller/front_right_wheel_joint + - pid_controller/front_left_wheel_joint + - pid_controller/rear_right_wheel_joint + - pid_controller/rear_left_wheel_joint + steers_names: + - pid_controller/front_right_steering_joint + - pid_controller/front_left_steering_joint + - pid_controller/rear_right_steering_joint + - pid_controller/rear_left_steering_joint + wheels_state_names: + - front_right_wheel_joint + - front_left_wheel_joint + - rear_right_wheel_joint + - rear_left_wheel_joint + steers_state_names: + - front_right_steering_joint + - front_left_steering_joint + - rear_right_steering_joint + - rear_left_steering_joint + wheelbase: 3.24644 + wheel_track: 2.12321 + wheels_radius: 0.45 diff --git a/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp new file mode 100644 index 0000000000..09d3a41ddd --- /dev/null +++ b/swerve_drive_controller/test/test_load_swerve_drive_controller.cpp @@ -0,0 +1,49 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "rclcpp/executor.hpp" +#include "rclcpp/executors/single_threaded_executor.hpp" +#include "rclcpp/utilities.hpp" +#include "ros2_control_test_assets/descriptions.hpp" + +TEST(TestLoadSwerveDriveController, load_controller) +{ + std::shared_ptr executor = + std::make_shared(); + + controller_manager::ControllerManager cm( + std::make_unique( + ros2_control_test_assets::minimal_robot_urdf), + executor, "test_controller_manager"); + + ASSERT_NE( + cm.load_controller( + "test_swerve_drive_controller", + "swerve_drive_controller/SwerveDriveController"), + nullptr); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.cpp b/swerve_drive_controller/test/test_swerve_drive_controller.cpp new file mode 100644 index 0000000000..072585f60b --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.cpp @@ -0,0 +1,367 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_swerve_drive_controller.hpp" + +#include +#include +#include +#include +#include + +class SwerveDriveControllerTest +: public SwerveDriveControllerFixture +{ +}; + +TEST_F(SwerveDriveControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.wheels_names, testing::ElementsAreArray(wheels_names_)); + ASSERT_THAT( + controller_->params_.steers_names, testing::ElementsAreArray(steers_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_->swerve_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->swerve_params_.wheels_radius, wheels_radius_); + ASSERT_EQ(controller_->swerve_params_.wheel_track, wheel_track_); +} + +TEST_F(SwerveDriveControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + 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_FRONT_RIGHT_WHEEL], + wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_FRONT_LEFT_WHEEL], + wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_REAR_RIGHT_WHEEL], + wheels_names_[2] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_REAR_LEFT_WHEEL], + wheels_names_[3] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_FRONT_RIGHT_WHEEL], + steers_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_FRONT_LEFT_WHEEL], + steers_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_REAR_RIGHT_WHEEL], + steers_names_[2] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_REAR_LEFT_WHEEL], + steers_names_[3] + "/" + 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_FRONT_RIGHT_WHEEL], + controller_->wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_FRONT_LEFT_WHEEL], + controller_->wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_REAR_RIGHT_WHEEL], + controller_->wheels_state_names_[2] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_REAR_LEFT_WHEEL], + controller_->wheels_state_names_[3] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_FRONT_RIGHT_WHEEL], + controller_->steers_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_FRONT_LEFT_WHEEL], + controller_->steers_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_REAR_RIGHT_WHEEL], + controller_->steers_state_names_[2] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_REAR_LEFT_WHEEL], + controller_->steers_state_names_[3] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfsTIME + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +TEST_F(SwerveDriveControllerTest, activate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + // check that the message is reset + auto msg = controller_->input_ref_.readFromNonRT(); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.linear.z)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.x)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.y)); + EXPECT_TRUE(std::isnan((*msg)->twist.angular.z)); +} + +TEST_F(SwerveDriveControllerTest, update_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(SwerveDriveControllerTest, deactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); +} + +TEST_F(SwerveDriveControllerTest, reactivate_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(std::isnan(controller_->command_interfaces_[0].get_value())); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); +} + +TEST_F(SwerveDriveControllerTest, test_update_logic) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(false); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_FALSE(controller_->is_in_chained_mode()); + + // set command statically + std::shared_ptr msg = std::make_shared(); + msg->header.stamp = controller_->get_node()->now(); + msg->twist.linear.x = 0.1; + msg->twist.angular.z = 0.2; + controller_->input_ref_.writeFromNonRT(msg); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_FRONT_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_FRONT_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_REAR_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_REAR_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_FRONT_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_FRONT_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_REAR_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_REAR_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + EXPECT_FALSE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(SwerveDriveControllerTest, test_update_logic_chained) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + controller_->set_chained_mode(true); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_TRUE(controller_->is_in_chained_mode()); + + controller_->reference_interfaces_[0] = 0.1; + controller_->reference_interfaces_[1] = 0.2; + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_FRONT_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_FRONT_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_REAR_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_TRACTION_REAR_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_FRONT_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_FRONT_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_REAR_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[STATE_STEER_REAR_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + + EXPECT_TRUE(std::isnan((*(controller_->input_ref_.readFromRT()))->twist.linear.x)); + EXPECT_EQ(controller_->reference_interfaces_.size(), joint_reference_interfaces_.size()); + for (const auto & interface : controller_->reference_interfaces_) + { + EXPECT_TRUE(std::isnan(interface)); + } +} + +TEST_F(SwerveDriveControllerTest, receive_message_and_publish_updated_status) +{ + SetUpController(); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(controller_->get_node()->get_node_base_interface()); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + ControllerStateMsg msg; + subscribe_and_get_messages(msg); + + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_FRONT_RIGHT_WHEEL], 1.1); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_FRONT_LEFT_WHEEL], 3.3); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_REAR_RIGHT_WHEEL], 2.2); + EXPECT_EQ(msg.linear_velocity_command[STATE_TRACTION_REAR_LEFT_WHEEL], 4.4); + EXPECT_EQ(msg.steering_angle_command[0], 5.5); + EXPECT_EQ(msg.steering_angle_command[1], 6.6); + EXPECT_EQ(msg.steering_angle_command[2], 7.7); + EXPECT_EQ(msg.steering_angle_command[3], 8.8); + + publish_commands(); + ASSERT_TRUE(controller_->wait_for_commands(executor)); + + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_FRONT_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_FRONT_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_REAR_RIGHT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_TRACTION_REAR_LEFT_WHEEL].get_value(), 0.22222222222222224, + COMMON_THRESHOLD); + + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_FRONT_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_FRONT_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_REAR_RIGHT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + EXPECT_NEAR( + controller_->command_interfaces_[CMD_STEER_REAR_LEFT_WHEEL].get_value(), 1.4179821977774734, + COMMON_THRESHOLD); + + + subscribe_and_get_messages(msg); + + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_FRONT_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_FRONT_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_REAR_RIGHT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + EXPECT_NEAR( + msg.linear_velocity_command[CMD_TRACTION_REAR_LEFT_WHEEL], 0.22222222222222224, COMMON_THRESHOLD); + + + EXPECT_NEAR(msg.steering_angle_command[0], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[1], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[2], 1.4179821977774734, COMMON_THRESHOLD); + EXPECT_NEAR(msg.steering_angle_command[3], 1.4179821977774734, COMMON_THRESHOLD); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/swerve_drive_controller/test/test_swerve_drive_controller.hpp b/swerve_drive_controller/test/test_swerve_drive_controller.hpp new file mode 100644 index 0000000000..bfcddea61c --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -0,0 +1,372 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_SWERVE_DRIVE_CONTROLLER_HPP_ +#define TEST_SWERVE_DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "swerve_drive_controller/swerve_drive_controller.hpp" +#include "gmock/gmock.h" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/utilities.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" + +using ControllerStateMsg = + steering_controllers_library::SteeringControllersLibrary::AckermanControllerState; +using ControllerReferenceMsg = + steering_controllers_library::SteeringControllersLibrary::ControllerTwistReferenceMsg; + +// name constants for state interfaces +using swerve_drive_controller::STATE_STEER_FRONT_LEFT_WHEEL; +using swerve_drive_controller::STATE_STEER_FRONT_RIGHT_WHEEL; +using swerve_drive_controller::STATE_STEER_REAR_LEFT_WHEEL; +using swerve_drive_controller::STATE_STEER_REAR_RIGHT_WHEEL; +using swerve_drive_controller::STATE_TRACTION_FRONT_LEFT_WHEEL; +using swerve_drive_controller::STATE_TRACTION_FRONT_RIGHT_WHEEL; +using swerve_drive_controller::STATE_TRACTION_REAR_LEFT_WHEEL; +using swerve_drive_controller::STATE_TRACTION_REAR_RIGHT_WHEEL; + +// name constants for command interfaces +using swerve_drive_controller::CMD_STEER_FRONT_LEFT_WHEEL; +using swerve_drive_controller::CMD_STEER_FRONT_RIGHT_WHEEL; +using swerve_drive_controller::CMD_STEER_REAR_LEFT_WHEEL; +using swerve_drive_controller::CMD_STEER_REAR_RIGHT_WHEEL; +using swerve_drive_controller::CMD_TRACTION_FRONT_LEFT_WHEEL; +using swerve_drive_controller::CMD_TRACTION_FRONT_RIGHT_WHEEL; +using swerve_drive_controller::CMD_TRACTION_REAR_LEFT_WHEEL; +using swerve_drive_controller::CMD_TRACTION_REAR_RIGHT_WHEEL; + +namespace +{ +constexpr auto NODE_SUCCESS = controller_interface::CallbackReturn::SUCCESS; +constexpr auto NODE_ERROR = controller_interface::CallbackReturn::ERROR; +const double COMMON_THRESHOLD = 1e-6; +} // namespace + +// subclassing and friending so we can access member variables +class TestableSwerveDriveController +: public swerve_drive_controller::SwerveDriveController +{ + FRIEND_TEST(SwerveDriveControllerTest, all_parameters_set_configure_success); + FRIEND_TEST(SwerveDriveControllerTest, check_exported_interfaces); + FRIEND_TEST(SwerveDriveControllerTest, activate_success); + FRIEND_TEST(SwerveDriveControllerTest, update_success); + FRIEND_TEST(SwerveDriveControllerTest, deactivate_success); + FRIEND_TEST(SwerveDriveControllerTest, reactivate_success); + FRIEND_TEST(SwerveDriveControllerTest, test_update_logic); + FRIEND_TEST(SwerveDriveControllerTest, test_update_logic_chained); + FRIEND_TEST(SwerveDriveControllerTest, publish_status_success); + FRIEND_TEST(SwerveDriveControllerTest, receive_message_and_publish_updated_status); + +public: + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override + { + auto ret = + swerve_drive_controller::SwerveDriveController::on_configure(previous_state); + // Only if on_configure is successful create subscription + if (ret == CallbackReturn::SUCCESS) + { + ref_subscriber_wait_set_.add_subscription(ref_subscriber_twist_); + } + return ret; + } + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override + { + auto ref_itfs = on_export_reference_interfaces(); + return swerve_drive_controller::SwerveDriveController::on_activate(previous_state); + } + + /** + * @brief wait_for_command blocks until a new ControllerReferenceMsg is received. + * Requires that the executor is not spinned elsewhere between the + * message publication and the call to this function. + * + * @return true if new ControllerReferenceMsg msg was received, false if timeout. + */ + bool wait_for_command( + rclcpp::Executor & executor, rclcpp::WaitSet & subscriber_wait_set, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + bool success = subscriber_wait_set.wait(timeout).kind() == rclcpp::WaitResultKind::Ready; + if (success) + { + executor.spin_some(); + } + return success; + } + + bool wait_for_commands( + rclcpp::Executor & executor, + const std::chrono::milliseconds & timeout = std::chrono::milliseconds{500}) + { + return wait_for_command(executor, ref_subscriber_wait_set_, timeout); + } + +private: + rclcpp::WaitSet ref_subscriber_wait_set_; +}; + +// We are using template class here for easier reuse of Fixture in specializations of controllers +template +class SwerveDriveControllerFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() {} + + void SetUp() + { + // initialize controller + controller_ = std::make_unique(); + + command_publisher_node_ = std::make_shared("command_publisher"); + command_publisher_ = command_publisher_node_->create_publisher( + "/test_swerve_drive_controller/reference", rclcpp::SystemDefaultsQoS()); + } + + static void TearDownTestCase() {} + + void TearDown() { controller_.reset(nullptr); } + +protected: + void SetUpController(const std::string controller_name = "test_swerve_drive_controller") + { + ASSERT_EQ( + controller_->init(controller_name, "", 0, "", controller_->define_custom_node_options()), + controller_interface::return_type::OK); + + if (position_feedback_ == true) + { + traction_interface_name_ = "position"; + } + else + { + traction_interface_name_ = "velocity"; + } + + std::vector command_ifs; + command_itfs_.reserve(joint_command_values_.size()); + command_ifs.reserve(joint_command_values_.size()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheels_names_[0], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_FRONT_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheels_names_[1], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_FRONT_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheels_names_[2], traction_interface_name_, + &joint_command_values_[CMD_TRACTION_REAR_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + wheels_names_[3], steering_interface_name_, + &joint_command_values_[CMD_TRACTION_REAR_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + steers_names_[0], steering_interface_name_, + &joint_command_values_[CMD_STEER_FRONT_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + steers_names_[1], steering_interface_name_, + &joint_command_values_[CMD_STEER_FRONT_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + steers_names_[2], steering_interface_name_, + &joint_command_values_[CMD_STEER_REAR_RIGHT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + command_itfs_.emplace_back(hardware_interface::CommandInterface( + steers_names_[3], steering_interface_name_, + &joint_command_values_[CMD_STEER_REAR_LEFT_WHEEL])); + command_ifs.emplace_back(command_itfs_.back()); + + + std::vector state_ifs; + state_itfs_.reserve(joint_state_values_.size()); + state_ifs.reserve(joint_state_values_.size()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[0], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_FRONT_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[1], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_FRONT_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[2], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_REAR_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[3], traction_interface_name_, + &joint_state_values_[STATE_TRACTION_REAR_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[0], steering_interface_name_, + &joint_state_values_[STATE_STEER_FRONT_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[1], steering_interface_name_, + &joint_state_values_[STATE_STEER_FRONT_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[2], steering_interface_name_, + &joint_state_values_[STATE_STEER_REAR_RIGHT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + state_itfs_.emplace_back(hardware_interface::StateInterface( + wheels_names_[3], steering_interface_name_, + &joint_state_values_[STATE_STEER_REAR_LEFT_WHEEL])); + state_ifs.emplace_back(state_itfs_.back()); + + + controller_->assign_interfaces(std::move(command_ifs), std::move(state_ifs)); + } + + void subscribe_and_get_messages(ControllerStateMsg & msg) + { + // create a new subscriber + rclcpp::Node test_subscription_node("test_subscription_node"); + auto subs_callback = [&](const ControllerStateMsg::SharedPtr) {}; + auto subscription = test_subscription_node.create_subscription( + "/test_swerve_drive_controller/controller_state", 10, subs_callback); + + // call update to publish the test value + ASSERT_EQ( + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)), + controller_interface::return_type::OK); + + // call update to publish the test value + // since update doesn't guarantee a published message, republish until received + int max_sub_check_loop_count = 5; // max number of tries for pub/sub loop + rclcpp::WaitSet wait_set; // block used to wait on message + wait_set.add_subscription(subscription); + while (max_sub_check_loop_count--) + { + controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)); + // check if message has been received + if (wait_set.wait(std::chrono::milliseconds(2)).kind() == rclcpp::WaitResultKind::Ready) + { + break; + } + } + ASSERT_GE(max_sub_check_loop_count, 0) << "Test was unable to publish a message through " + "controller/broadcaster update loop"; + + // take message from subscription + rclcpp::MessageInfo msg_info; + ASSERT_TRUE(subscription->take(msg, msg_info)); + } + + void publish_commands(const double linear = 0.1, const double angular = 0.2) + { + auto wait_for_topic = [&](const auto topic_name) + { + size_t wait_count = 0; + while (command_publisher_node_->count_subscribers(topic_name) == 0) + { + if (wait_count >= 5) + { + auto error_msg = + std::string("publishing to ") + topic_name + " but no node subscribes to it"; + throw std::runtime_error(error_msg); + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ++wait_count; + } + }; + + wait_for_topic(command_publisher_->get_topic_name()); + + ControllerReferenceMsg msg; + msg.twist.linear.x = linear; + msg.twist.angular.z = angular; + + command_publisher_->publish(msg); + } + +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; + bool use_stamped_vel_ = true; + std::vector wheels_names_ = {"front_right_wheel_joint", "front_left_wheel_joint", "rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steers_names_ = { + "front_right_steering_joint", "front_left_steering_joint", "rear_right_steering_joint", "rear_left_steering_joint"}; + std::vector joint_names_ = { + wheels_names_[0], wheels_names_[1], wheels_names_[2], wheels_names_[3], + steers_names_[0], steers_names_[1], steers_names_[2], steers_names_[3], + }; + + std::vector wheels_preceeding_names_ = { + "pid_controller/front_right_wheel_joint", "pid_controller/front_left_wheel_joint", "pid_controller/rear_right_wheel_joint", "pid_controller/rear_left_wheel_joint"}; + std::vector steers_preceeding_names_ = { + "pid_controller/front_right_steering_joint", "pid_controller/front_left_steering_joint", "pid_controller/rear_right_steering_joint", "pid_controller/rear_left_steering_joint"}; + std::vector preceeding_joint_names_ = { + wheels_preceeding_names_[0], wheels_preceeding_names_[1], wheels_preceeding_names_[2], wheels_preceeding_names_[3], + steers_preceeding_names_[0], steers_preceeding_names_[1], steers_preceeding_names_[2], steers_preceeding_names_[3]}; + + double wheelbase_ = 3.24644; + double wheel_track_ = 2.12321; + double wheels_radius_ = 0.45; + + std::array joint_state_values_ = {0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0}; + std::array joint_command_values_ = {1.1, 3.3, 2.2, 4.4, 5.5, 6.6, 7.7, 8.8}; + std::array joint_reference_interfaces_ = {"linear/velocity", "angular/position"}; + std::string steering_interface_name_ = "position"; + // defined in setup + std::string traction_interface_name_ = ""; + std::string preceeding_prefix_ = "pid_controller"; + + std::vector state_itfs_; + std::vector command_itfs_; + + // Test related parameters + std::unique_ptr controller_; + rclcpp::Node::SharedPtr command_publisher_node_; + rclcpp::Publisher::SharedPtr command_publisher_; +}; + +#endif // TEST_SWERVE_DRIVE_CONTROLLER_HPP_ diff --git a/swerve_drive_controller/test/test_swerve_drive_controller_preceeding.cpp b/swerve_drive_controller/test/test_swerve_drive_controller_preceeding.cpp new file mode 100644 index 0000000000..1456ae56ee --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller_preceeding.cpp @@ -0,0 +1,133 @@ +// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "test_swerve_drive_controller.hpp" + +#include +#include +#include +#include +#include + +class SwerveDriveControllerTest +: public SwerveDriveControllerFixture +{ +}; + +TEST_F(SwerveDriveControllerTest, all_parameters_set_configure_success) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + ASSERT_THAT( + controller_->params_.wheels_names, + testing::ElementsAreArray(wheels_preceeding_names_)); + ASSERT_THAT( + controller_->params_.steers_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_->swerve_params_.wheelbase, wheelbase_); + ASSERT_EQ(controller_->swerve_params_.wheels_radius, wheels_radius_); + ASSERT_EQ(controller_->swerve_params_.wheel_track, wheel_track_); +} + +TEST_F(SwerveDriveControllerTest, check_exported_interfaces) +{ + SetUpController(); + + ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); + + 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_FRONT_RIGHT_WHEEL], + preceeding_prefix_ + "/" + wheels_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_FRONT_LEFT_WHEEL], + preceeding_prefix_ + "/" + wheels_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_REAR_RIGHT_WHEEL], + preceeding_prefix_ + "/" + wheels_names_[2] + "/" + traction_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_TRACTION_REAR_LEFT_WHEEL], + preceeding_prefix_ + "/" + wheels_names_[3] + "/" + traction_interface_name_); + + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_FRONT_RIGHT_WHEEL], + preceeding_prefix_ + "/" + steers_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_FRONT_LEFT_WHEEL], + preceeding_prefix_ + "/" + steers_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_REAR_RIGHT_WHEEL], + preceeding_prefix_ + "/" + steers_names_[2] + "/" + steering_interface_name_); + EXPECT_EQ( + cmd_if_conf.names[CMD_STEER_REAR_LEFT_WHEEL], + preceeding_prefix_ + "/" + steers_names_[3] + "/" + 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_FRONT_RIGHT_WHEEL], + controller_->wheels_state_names_[0] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_FRONT_LEFT_WHEEL], + controller_->wheels_state_names_[1] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_REAR_RIGHT_WHEEL], + controller_->wheels_state_names_[2] + "/" + traction_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_TRACTION_REAR_LEFT_WHEEL], + controller_->wheels_state_names_[3] + "/" + traction_interface_name_); + + EXPECT_EQ( + state_if_conf.names[STATE_STEER_FRONT_RIGHT_WHEEL], + controller_->steers_state_names_[0] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_FRONT_LEFT_WHEEL], + controller_->steers_state_names_[1] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_REAR_RIGHT_WHEEL], + controller_->steers_state_names_[2] + "/" + steering_interface_name_); + EXPECT_EQ( + state_if_conf.names[STATE_STEER_REAR_LEFT_WHEEL], + controller_->steers_state_names_[3] + "/" + steering_interface_name_); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + // check ref itfs + auto reference_interfaces = controller_->export_reference_interfaces(); + ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size()); + for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i) + { + const std::string ref_itf_name = + std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i]; + EXPECT_EQ(reference_interfaces[i].get_name(), ref_itf_name); + EXPECT_EQ(reference_interfaces[i].get_prefix_name(), controller_->get_node()->get_name()); + EXPECT_EQ(reference_interfaces[i].get_interface_name(), joint_reference_interfaces_[i]); + } +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/tricycle_steering_controller/src/tricycle_steering_controller.cpp b/tricycle_steering_controller/src/tricycle_steering_controller.cpp index 03be6b88f0..12079d9cd0 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.cpp +++ b/tricycle_steering_controller/src/tricycle_steering_controller.cpp @@ -30,20 +30,11 @@ 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 traction_wheels_radius = tricycle_params_.traction_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); - } - else - { - odometry_.set_wheel_params(front_wheels_radius, wheelbase, wheel_track); - } - + 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..8dbd085408 100644 --- a/tricycle_steering_controller/src/tricycle_steering_controller.yaml +++ b/tricycle_steering_controller/src/tricycle_steering_controller.yaml @@ -21,22 +21,11 @@ tricycle_steering_controller: } } - front_wheels_radius: + traction_wheels_radius: { type: double, default_value: 0.0, - description: "Front wheels radius.", - read_only: false, - validation: { - gt<>: [0.0] - } - } - - rear_wheels_radius: - { - type: double, - default_value: 0.0, - description: "Rear wheels radius.", + description: "Traction wheels radius.", read_only: false, validation: { gt<>: [0.0] diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.cpp index c555de53de..34bc318c24 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_.wheels_names, testing::ElementsAreArray(wheels_names_)); ASSERT_THAT( - controller_->params_.front_wheels_names, testing::ElementsAreArray(front_wheels_names_)); - ASSERT_EQ(controller_->params_.front_steering, front_steering_); + controller_->params_.steers_names, testing::ElementsAreArray(steers_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_); + wheels_names_[0] + "/" + traction_interface_name_); EXPECT_EQ( cmd_if_conf.names[CMD_TRACTION_LEFT_WHEEL], - rear_wheels_names_[1] + "/" + traction_interface_name_); + wheels_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], steers_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_->wheels_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_->wheels_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_->steers_state_names_[0] + "/" + steering_interface_name_); EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); // check ref itfs diff --git a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp index 1807ab59a8..748b702629 100644 --- a/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp +++ b/tricycle_steering_controller/test/test_tricycle_steering_controller.hpp @@ -164,17 +164,17 @@ 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_, + wheels_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_, + wheels_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])); + steers_names_[0], steering_interface_name_, &joint_command_values_[CMD_STEER_WHEEL])); command_ifs.emplace_back(command_itfs_.back()); std::vector state_ifs; @@ -182,17 +182,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_, + wheels_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_, + wheels_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])); + steers_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 +261,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 wheels_names_ = {"rear_right_wheel_joint", "rear_left_wheel_joint"}; + std::vector steers_names_ = {"steering_axis_joint"}; std::vector joint_names_ = { - rear_wheels_names_[0], rear_wheels_names_[1], front_wheels_names_[0]}; + wheels_names_[0], wheels_names_[1], steers_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..41f25e8348 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_.wheels_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_.steers_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_ + "/" + wheels_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_ + "/" + wheels_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_ + "/" + steers_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_->wheels_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_->wheels_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_->steers_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..e25bc0817f 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] + wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steers_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..4aa95265b5 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] + wheels_names: [pid_controller/rear_right_wheel_joint, pid_controller/rear_left_wheel_joint] + steers_names: [pid_controller/steering_axis_joint] + wheels_state_names: [rear_right_wheel_joint, rear_left_wheel_joint] + steers_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