From b2605a128148268e2f698fd2a98b3318461c5c82 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 17 Jun 2024 13:02:46 +0200 Subject: [PATCH] Fix mixing of joints and fix to velocity only interface. --- .../mecanum_drive_controller.hpp | 18 ++++- .../src/mecanum_drive_controller.cpp | 56 +++++++------- .../src/mecanum_drive_controller.yaml | 74 +++++++++++++++---- .../test/mecanum_drive_controller_params.yaml | 14 +--- ...um_drive_controller_preceeding_params.yaml | 19 ++--- .../test/test_mecanum_drive_controller.cpp | 27 +++++-- .../test/test_mecanum_drive_controller.hpp | 20 +++-- ...st_mecanum_drive_controller_preceeding.cpp | 28 +++++-- 8 files changed, 174 insertions(+), 82 deletions(-) diff --git a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp index 144b295610..847483cfc9 100644 --- a/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/include/mecanum_drive_controller/mecanum_drive_controller.hpp @@ -92,7 +92,23 @@ class MecanumDriveController : public controller_interface::ChainableControllerI std::shared_ptr param_listener_; mecanum_drive_controller::Params params_; - // used for chained controller + /// Internal lists with joint names. + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ + std::vector command_joint_names_; + /// Internal lists with joint names. + /** + * The list is sorted in the following order: + * - front left wheel + * - front right wheel + * - back right wheel + * - back left wheel + */ std::vector state_joint_names_; // Names of the references, ex: high level vel commands from MoveIt, Nav2, etc. diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index 89b2b6f289..afaad73291 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,24 +74,24 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - if (!params_.state_joint_names.empty()) + auto prepare_lists_with_joint_names = [&command_joints = this->command_joint_names_, &state_joints = this->state_joint_names_](const std::string & command_joint_name, const std::string & state_joint_name) { - state_joint_names_ = params_.state_joint_names; - } - else - { - state_joint_names_ = params_.command_joint_names; - } + command_joints.push_back(command_joint_name); + if (state_joint_name.empty()) + { + state_joints.push_back(command_joint_name); + } + else + { + state_joints.push_back(state_joint_name); + } + }; - if (params_.command_joint_names.size() != state_joint_names_.size()) - { - RCLCPP_FATAL( - get_node()->get_logger(), - "Size of 'joints' (%ld) and 'state_joint_names' (%ld) parameters has " - "to be the same!", - params_.command_joint_names.size(), state_joint_names_.size()); - return CallbackReturn::FAILURE; - } + // The joint names are sorted accoring to the order documented in the header file! + prepare_lists_with_joint_names(params_.front_left_wheel_command_joint_name, params_.front_left_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.front_right_wheel_command_joint_name, params_.front_right_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.rear_right_wheel_command_joint_name, params_.rear_right_wheel_state_joint_name); + prepare_lists_with_joint_names(params_.rear_left_wheel_command_joint_name, params_.rear_left_wheel_state_joint_name); // Set wheel params for the odometry computation odometry_.setWheelsParams( @@ -229,10 +229,10 @@ MecanumDriveController::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(params_.command_joint_names.size()); - for (const auto & joint : params_.command_joint_names) + command_interfaces_config.names.reserve(command_joint_names_.size()); + for (const auto & joint : command_joint_names_) { - command_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + command_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); } return command_interfaces_config; @@ -248,7 +248,7 @@ controller_interface::InterfaceConfiguration MecanumDriveController::state_inter for (const auto & joint : state_joint_names_) { - state_interfaces_config.names.push_back(joint + "/" + params_.interface_name); + state_interfaces_config.names.push_back(joint + "/" + hardware_interface::HW_IF_VELOCITY); } return state_interfaces_config; @@ -393,32 +393,32 @@ controller_interface::return_type MecanumDriveController::update_and_write_comma linear_trans_from_base_to_center.x() * reference_interfaces_[2]; velocity_in_center_frame_angular_z_ = reference_interfaces_[2]; - double w_front_left_vel = + const double w_front_left_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_back_left_vel = + const double w_front_right_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_back_right_vel = + const double w_back_right_vel = 1.0 / params_.kinematics.wheels_radius * (velocity_in_center_frame_linear_x_ - velocity_in_center_frame_linear_y_ + params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); - double w_front_right_vel = + const double w_back_left_vel = 1.0 / params_.kinematics.wheels_radius * - (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ + + (velocity_in_center_frame_linear_x_ + velocity_in_center_frame_linear_y_ - params_.kinematics.sum_of_robot_center_projection_on_X_Y_axis * velocity_in_center_frame_angular_z_); // Set wheels velocities: command_interfaces_[0].set_value(w_front_left_vel); - command_interfaces_[1].set_value(w_back_left_vel); + command_interfaces_[1].set_value(w_front_right_vel); command_interfaces_[2].set_value(w_back_right_vel); - command_interfaces_[3].set_value(w_front_right_vel); + command_interfaces_[3].set_value(w_back_left_vel); } else { diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.yaml b/mecanum_drive_controller/src/mecanum_drive_controller.yaml index 225f62414f..88627c0f60 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.yaml +++ b/mecanum_drive_controller/src/mecanum_drive_controller.yaml @@ -5,24 +5,73 @@ mecanum_drive_controller: description: "Timeout for controller references after which they will be reset. This is especially useful for controllers that can cause unwanted and dangerous behavior if reference is not reset, e.g., velocity controllers. If value is 0 the reference is reset after each run.", } - command_joint_names: { - type: string_array, - default_value: [], - description: "Name of the wheels joints.", - read_only: false, + # Command joint names + front_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + front_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding front right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } } - state_joint_names: { - type: string_array, - default_value: [], - description: "(optional) Specifies joints for reading states. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + rear_right_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear right wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + rear_left_wheel_command_joint_name: { + type: string, + default_value: "", + description: "Name of the joints for commanding rear left wheel.", + read_only: true, + validation: { + not_empty<>: null, + } + } + + # State joint names + front_left_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + front_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of front right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", + read_only: true, + } + + rear_right_wheel_state_joint_name: { + type: string, + default_value: "", + description: "(optional) Specifies a joint name for reading states of rear right wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, } - interface_name: { + rear_left_wheel_state_joint_name: { type: string, default_value: "", - description: "Name of the interface used by the controller for sending commands, reading states and getting references.", + description: "(optional) Specifies a joint name for reading states of rear left wheel. This parameter is only relevant when state joints are different then command joint, i.e., when a following controller is used.", read_only: true, } @@ -57,8 +106,7 @@ mecanum_drive_controller: sum_of_robot_center_projection_on_X_Y_axis: { type: double, default_value: 0.0, - description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on - the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", + description: "Wheels geometric param used in mecanum wheels' IK. lx and ly represent the distance from the robot's center to the wheels projected on the x and y axis with origin at robots center respectively, sum_of_robot_center_projection_on_X_Y_axis = lx+ly", read_only: false, } diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml index 9eec559dd1..bc97335dd5 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_params.yaml @@ -1,25 +1,19 @@ test_mecanum_drive_controller: ros__parameters: - reference_timeout: 0.1 - command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - - interface_name: velocity + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" kinematics: base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml index 459a515eb1..c4e5bb391a 100644 --- a/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml +++ b/mecanum_drive_controller/test/mecanum_drive_controller_preceeding_params.yaml @@ -1,27 +1,24 @@ test_mecanum_drive_controller: ros__parameters: - reference_timeout: 0.1 - command_joint_names: ["front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", "front_right_wheel_joint"] - - state_joint_names: ["state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", "state_front_right_wheel_joint"] + front_left_wheel_command_joint_name: "front_left_wheel_joint" + front_right_wheel_command_joint_name: "front_right_wheel_joint" + rear_right_wheel_command_joint_name: "back_right_wheel_joint" + rear_left_wheel_command_joint_name: "back_left_wheel_joint" - interface_name: velocity + front_left_wheel_state_joint_name: "state_front_left_wheel_joint" + front_right_wheel_state_joint_name: "state_front_right_wheel_joint" + rear_right_wheel_state_joint_name: "state_back_right_wheel_joint" + rear_left_wheel_state_joint_name: "state_back_left_wheel_joint" kinematics: base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 } - wheels_radius: 0.5 - sum_of_robot_center_projection_on_X_Y_axis: 1.0 base_frame_id: "base_link" - odom_frame_id: "odom" - enable_odom_tf: true - twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] - pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0] diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 34009f2d7f..83ec710741 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -49,9 +49,14 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_TRUE(controller_->params_.command_joint_names.empty()); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); @@ -63,10 +68,18 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.y, 0.0); ASSERT_EQ(controller_->params_.kinematics.base_frame_offset.theta, 0.0); - ASSERT_THAT( - controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); + ASSERT_THAT(controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + // When state joint names are empty they are the same as the command joint names + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(command_joint_names_)); } TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_exported_interfaces) diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp index dd9a7b358c..d2c0a79b0f 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.hpp @@ -27,6 +27,7 @@ #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "mecanum_drive_controller/mecanum_drive_controller.hpp" #include "rclcpp/parameter_value.hpp" #include "rclcpp/time.hpp" @@ -259,14 +260,23 @@ class MecanumDriveControllerFixture : public ::testing::Test protected: std::vector reference_interface_names = { "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"}; + + static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_CMD_JOINT_NAME[] = "back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_CMD_JOINT_NAME[] = "back_left_wheel_joint"; std::vector command_joint_names_ = { - "front_left_wheel_joint", "back_left_wheel_joint", "back_right_wheel_joint", - "front_right_wheel_joint"}; + TEST_FRONT_LEFT_CMD_JOINT_NAME, TEST_FRONT_RIGHT_CMD_JOINT_NAME, TEST_REAR_RIGHT_CMD_JOINT_NAME, + TEST_REAR_LEFT_CMD_JOINT_NAME}; + static constexpr char TEST_FRONT_LEFT_STATE_JOINT_NAME[] = "state_front_left_wheel_joint"; + static constexpr char TEST_FRONT_RIGHT_STATE_JOINT_NAME[] = "state_front_right_wheel_joint"; + static constexpr char TEST_REAR_RIGHT_STATE_JOINT_NAME[] = "state_back_right_wheel_joint"; + static constexpr char TEST_REAR_LEFT_STATE_JOINT_NAME[] = "state_back_left_wheel_joint"; std::vector state_joint_names_ = { - "state_front_left_wheel_joint", "state_back_left_wheel_joint", "state_back_right_wheel_joint", - "state_front_right_wheel_joint"}; - std::string interface_name_ = "velocity"; + TEST_FRONT_LEFT_STATE_JOINT_NAME, TEST_FRONT_RIGHT_STATE_JOINT_NAME, + TEST_REAR_RIGHT_STATE_JOINT_NAME, TEST_REAR_LEFT_STATE_JOINT_NAME}; + std::string interface_name_ = hardware_interface::HW_IF_VELOCITY; // Controller-related parameters diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp index 5e8dcfefa1..232e902429 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -34,19 +34,33 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para SetUpController(); ASSERT_EQ(controller_->params_.reference_timeout, 0.0); - ASSERT_TRUE(controller_->params_.command_joint_names.empty()); - ASSERT_TRUE(controller_->params_.state_joint_names.empty()); - ASSERT_TRUE(controller_->params_.interface_name.empty()); + + ASSERT_TRUE(controller_->params_.front_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_command_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_left_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.front_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_right_wheel_state_joint_name.empty()); + ASSERT_TRUE(controller_->params_.rear_left_wheel_state_joint_name.empty()); ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS); ASSERT_EQ(controller_->params_.reference_timeout, 0.1); + + ASSERT_EQ(controller_->params_.front_left_wheel_command_joint_name, TEST_FRONT_LEFT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_command_joint_name, TEST_FRONT_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_command_joint_name, TEST_REAR_RIGHT_CMD_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_command_joint_name, TEST_REAR_LEFT_CMD_JOINT_NAME); ASSERT_THAT( - controller_->params_.command_joint_names, testing::ElementsAreArray(command_joint_names_)); + controller_->command_joint_names_, testing::ElementsAreArray(command_joint_names_)); + + ASSERT_EQ(controller_->params_.front_left_wheel_state_joint_name, TEST_FRONT_LEFT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.front_right_wheel_state_joint_name, TEST_FRONT_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_right_wheel_state_joint_name, TEST_REAR_RIGHT_STATE_JOINT_NAME); + ASSERT_EQ(controller_->params_.rear_left_wheel_state_joint_name, TEST_REAR_LEFT_STATE_JOINT_NAME); ASSERT_THAT( - controller_->params_.state_joint_names, testing::ElementsAreArray(state_joint_names_)); - ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); - ASSERT_EQ(controller_->params_.interface_name, interface_name_); + controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); } // checking if all interfaces, command and state interfaces are exported as expected