From 715c8a0ba162486c7b8195a020e5f6b620862b5a Mon Sep 17 00:00:00 2001 From: Quique Llorente Date: Mon, 6 May 2024 11:47:39 +0200 Subject: [PATCH] [STEERING]: Add swerve drive Signed-off-by: Quique Llorente --- .../steering_controllers_library.hpp | 4 +- .../steering_odometry.hpp | 1 + .../src/steering_controllers_library.cpp | 111 ++---- .../src/steering_controllers_library.yaml | 23 +- .../src/steering_odometry.cpp | 35 ++ .../steering_controllers_library_params.yaml | 4 +- .../test_steering_controllers_library.cpp | 16 +- .../test_steering_controllers_library.hpp | 45 +-- .../test/test_steering_odometry.cpp | 53 +++ swerve_drive_controller/CHANGELOG.rst | 194 +++++++++ swerve_drive_controller/CMakeLists.txt | 106 +++++ swerve_drive_controller/doc/userdoc.rst | 24 ++ .../swerve_drive_controller.hpp | 73 ++++ .../visibility_control.h | 52 +++ swerve_drive_controller/package.xml | 37 ++ .../src/swerve_drive_controller.cpp | 91 +++++ .../src/swerve_drive_controller.yaml | 24 ++ .../swerve_drive_controller.xml | 8 + .../test/swerve_drive_controller_params.yaml | 14 + ...ve_drive_controller_preceeding_params.yaml | 30 ++ .../test_load_swerve_drive_controller.cpp | 49 +++ .../test/test_swerve_drive_controller.cpp | 367 +++++++++++++++++ .../test/test_swerve_drive_controller.hpp | 370 ++++++++++++++++++ ...est_swerve_drive_controller_preceeding.cpp | 133 +++++++ 24 files changed, 1726 insertions(+), 138 deletions(-) create mode 100644 swerve_drive_controller/CHANGELOG.rst create mode 100644 swerve_drive_controller/CMakeLists.txt create mode 100644 swerve_drive_controller/doc/userdoc.rst create mode 100644 swerve_drive_controller/include/swerve_drive_controller/swerve_drive_controller.hpp create mode 100644 swerve_drive_controller/include/swerve_drive_controller/visibility_control.h create mode 100644 swerve_drive_controller/package.xml create mode 100644 swerve_drive_controller/src/swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/src/swerve_drive_controller.yaml create mode 100644 swerve_drive_controller/swerve_drive_controller.xml create mode 100644 swerve_drive_controller/test/swerve_drive_controller_params.yaml create mode 100644 swerve_drive_controller/test/swerve_drive_controller_preceeding_params.yaml create mode 100644 swerve_drive_controller/test/test_load_swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/test/test_swerve_drive_controller.cpp create mode 100644 swerve_drive_controller/test/test_swerve_drive_controller.hpp create mode 100644 swerve_drive_controller/test/test_swerve_drive_controller_preceeding.cpp 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 b560e2a782..9b9a90604a 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 @@ -136,8 +136,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 e4a22f6d3b..da89a29bac 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 9bf9fa51d6..4546015942 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 @@ -286,34 +286,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; } @@ -328,33 +310,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; @@ -451,30 +417,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]); } } @@ -517,14 +466,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 b18cac5ae1..ee8ca567e5 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 6fb20478a4..29a12a6050 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -276,6 +276,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 = {alpha, alpha, alpha, alpha}; + } + 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(alpha); + double denominator_first_member = 2 * instantaneous_center * std::cos(alpha); + double denominator_second_member = wheel_track_ * std::sin(alpha); + + 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::cos(alpha_r); + double radius_l = instantaneous_center / std::cos(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 d200d34961..1d58e26804 100644 --- a/steering_controllers_library/test/steering_controllers_library_params.yaml +++ b/steering_controllers_library/test/steering_controllers_library_params.yaml @@ -7,8 +7,8 @@ test_steering_controllers_library: velocity_rolling_window_size: 10 position_feedback: false use_stamped_vel: true - 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 1284b72096..30f74a6dc4 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,30 +295,23 @@ 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; bool use_stamped_vel_ = true; - 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..f538f91fdc 100644 --- a/steering_controllers_library/test/test_steering_odometry.cpp +++ b/steering_controllers_library/test/test_steering_odometry.cpp @@ -144,3 +144,56 @@ TEST(TestSteeringOdometry, ackermann_odometry) EXPECT_NEAR(odom.get_x(), .1, 1e-3); EXPECT_NEAR(odom.get_heading(), .01, 1e-3); } + +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]); // right (outer) > left (inner) + EXPECT_GT(cmd0[0], 0); + EXPECT_GT(cmd0[2], cmd0[3]); // right (outer) > left (inner) + EXPECT_GT(cmd0[2], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_LT(cmd1[0], cmd1[1]); // right (outer) < left (inner) + EXPECT_GT(cmd1[0], 0); + EXPECT_LT(cmd1[2], cmd1[3]); // right (outer) < left (inner) + EXPECT_GT(cmd1[2], 0); +} + +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]); // right (inner) < left outer) + EXPECT_GT(cmd0[0], 0); + auto cmd1 = std::get<1>(cmd); // steer + EXPECT_GT(std::abs(cmd1[0]), std::abs(cmd1[1])); // abs right (inner) > abs left (outer) + EXPECT_LT(cmd1[0], 0); +} 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..d668a309ba --- /dev/null +++ b/swerve_drive_controller/CMakeLists.txt @@ -0,0 +1,106 @@ +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 -Wconversion) +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() + +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_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..ff1f9804a8 --- /dev/null +++ b/swerve_drive_controller/test/test_swerve_drive_controller.hpp @@ -0,0 +1,370 @@ +// 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), 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; +}