Skip to content

Commit

Permalink
Fix formatting and typos.
Browse files Browse the repository at this point in the history
  • Loading branch information
destogl committed Jun 17, 2024
1 parent b2605a1 commit 50cd77c
Show file tree
Hide file tree
Showing 5 changed files with 35 additions and 23 deletions.
2 changes: 1 addition & 1 deletion mecanum_drive_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@ States
,,,,,,,
- <joint_names[i]>/<interface_name> [double] # in [rad] or [rad/s]
..note ::

``joint_names[i]`` can be of ``state_joint_names`` parameter (if used), ``command_joint_names`` otherwise.


Expand Down
18 changes: 12 additions & 6 deletions mecanum_drive_controller/src/mecanum_drive_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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())
Expand All @@ -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(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_));

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 50cd77c

Please sign in to comment.