diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index bbd7e3dd75..e0cb4313d5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -56,10 +56,10 @@ tl::expected command_interface_type_combinations( } if ( - rsl::contains>(interface_types, "effort") && - !(interface_types.size() == 1 || (interface_types.size() == 2 && - rsl::contains>(interface_types, "position"))) - ) + rsl::contains>(interface_types, "effort") && + !(interface_types.size() == 1 || + (interface_types.size() == 2 && + rsl::contains>(interface_types, "position")))) { return tl::make_unexpected( "'effort' command interface has to be used alone or with a 'position' interface"); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 16f0f0e159..a14e47b8fd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -786,7 +786,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // effort only or effort and position command interfaces require position and velocity state if ( - has_effort_command_interface_ && + has_effort_command_interface_ && (params_.command_interfaces.size() == 1 || (params_.command_interfaces.size() == 2 && has_position_command_interface_)) && (!has_velocity_state_interface_ || !has_position_state_interface_)) @@ -1509,8 +1509,9 @@ bool JointTrajectoryController::validate_trajectory_msg( if (!has_effort_command_interface_ && !points[i].effort.empty()) { RCLCPP_ERROR( - get_node()->get_logger(), "Trajectories with effort fields are only supported for " - "controllers using the 'effort' command interface."); + get_node()->get_logger(), + "Trajectories with effort fields are only supported for " + "controllers using the 'effort' command interface."); return false; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a6073b11cf..5765317ee5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -658,9 +658,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun {{0.3, 0.4, 0.6}}, {{1.7, 1.8, 1.9}}, {{2.10, 2.11, 2.12}}}; std::vector> empty_effort; // *INDENT-ON* - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); - publish(time_from_start, points, rclcpp::Time(), {}, {}, + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); + publish( + time_from_start, points, rclcpp::Time(), {}, {}, has_effort_command_interface ? effort : empty_effort); traj_controller_->wait_for_trajectory(executor); @@ -746,7 +748,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]) + state_reference.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); - } + } } executor.cancel(); @@ -864,7 +866,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_GT(0.0, joint_eff_[2]); EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI) + - state_reference.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); + state_reference.effort[2], + joint_eff_[2], k_p * COMMON_THRESHOLD); } } @@ -1434,8 +1437,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) // Effort is not supported in trajectory message traj_msg = good_traj_msg; traj_msg.points[0].effort = {1.0, 2.0, 3.0}; - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); if (has_effort_command_interface) { EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); @@ -2087,9 +2091,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) {{0.0, 0.0, 0.0}}, {{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; std::vector> empty_effort; // *INDENT-ON* - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); - publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities, + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); + publish( + time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities, has_effort_command_interface ? points_effort : empty_effort); traj_controller_->wait_for_trajectory(executor); auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); @@ -2168,7 +2174,8 @@ INSTANTIATE_TEST_SUITE_P( PositionEffortTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( - std::vector({"position", "effort"}), std::vector({"position", "velocity"})), + std::vector({"position", "effort"}), + std::vector({"position", "velocity"})), std::make_tuple( std::vector({"position", "effort"}), std::vector({"position", "velocity", "acceleration"})))); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 379241e15e..569e594d74 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -600,7 +600,8 @@ class TrajectoryControllerTest : public ::testing::Test } void expectCommandPoint( - std::vector position, std::vector velocity = {0.0, 0.0, 0.0}, std::vector effort = {0.0, 0.0, 0.0}) + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}, + std::vector effort = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only)