diff --git a/mecanum_drive_controller/doc/userdoc.rst b/mecanum_drive_controller/doc/userdoc.rst index 3aca53a859..e58ba3c2b0 100644 --- a/mecanum_drive_controller/doc/userdoc.rst +++ b/mecanum_drive_controller/doc/userdoc.rst @@ -37,7 +37,7 @@ States ,,,,,,, - / [double] # in [rad] or [rad/s] ..note :: - + ``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise. diff --git a/mecanum_drive_controller/src/mecanum_drive_controller.cpp b/mecanum_drive_controller/src/mecanum_drive_controller.cpp index afaad73291..025c45bb2a 100644 --- a/mecanum_drive_controller/src/mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/src/mecanum_drive_controller.cpp @@ -74,7 +74,9 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( { params_ = param_listener_->get_params(); - 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) + 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) { command_joints.push_back(command_joint_name); if (state_joint_name.empty()) @@ -87,11 +89,15 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure( } }; - // 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); + // The joint names are sorted according 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( diff --git a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp index cfab634376..36d0b679f2 100644 --- a/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_load_mecanum_drive_controller.cpp @@ -36,8 +36,7 @@ TEST(TestLoadMecanumDriveController, when_loading_controller_expect_no_exception ASSERT_NE( cm.load_controller( - "test_mecanum_drive_controller", - "mecanum_drive_controller/MecanumDriveController"), + "test_mecanum_drive_controller", "mecanum_drive_controller/MecanumDriveController"), nullptr); rclcpp::shutdown(); diff --git a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp index 83ec710741..9d49e344dd 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller.cpp @@ -68,9 +68,12 @@ 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_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_.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_)); 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 232e902429..edbee8a702 100644 --- a/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp +++ b/mecanum_drive_controller/test/test_mecanum_drive_controller_preceeding.cpp @@ -48,19 +48,23 @@ TEST_F(MecanumDriveControllerTest, when_controller_is_configured_expect_all_para 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_.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_)); - - 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_THAT(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_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); + ASSERT_THAT(controller_->state_joint_names_, testing::ElementsAreArray(state_joint_names_)); } // checking if all interfaces, command and state interfaces are exported as expected