Skip to content

Commit

Permalink
pre-commit fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
vlad-touchlab committed Jul 11, 2024
1 parent bcbac7a commit e6e3374
Show file tree
Hide file tree
Showing 4 changed files with 28 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -56,10 +56,10 @@ tl::expected<void, std::string> command_interface_type_combinations(
}

if (
rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
!(interface_types.size() == 1 || (interface_types.size() == 2 &&
rsl::contains<std::vector<std::string>>(interface_types, "position")))
)
rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
!(interface_types.size() == 1 ||
(interface_types.size() == 2 &&
rsl::contains<std::vector<std::string>>(interface_types, "position"))))
{
return tl::make_unexpected(
"'effort' command interface has to be used alone or with a 'position' interface");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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_))
Expand Down Expand Up @@ -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;
}
}
Expand Down
29 changes: 18 additions & 11 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::vector<double>> 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);

Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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<std::vector<double>> 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));
Expand Down Expand Up @@ -2168,7 +2174,8 @@ INSTANTIATE_TEST_SUITE_P(
PositionEffortTrajectoryControllers, TrajectoryControllerTestParameterized,
::testing::Values(
std::make_tuple(
std::vector<std::string>({"position", "effort"}), std::vector<std::string>({"position", "velocity"})),
std::vector<std::string>({"position", "effort"}),
std::vector<std::string>({"position", "velocity"})),
std::make_tuple(
std::vector<std::string>({"position", "effort"}),
std::vector<std::string>({"position", "velocity", "acceleration"}))));
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -599,7 +599,8 @@ class TrajectoryControllerTest : public ::testing::Test
}

void expectCommandPoint(
std::vector<double> position, std::vector<double> velocity = {0.0, 0.0, 0.0}, std::vector<double> effort = {0.0, 0.0, 0.0})
std::vector<double> position, std::vector<double> velocity = {0.0, 0.0, 0.0},
std::vector<double> effort = {0.0, 0.0, 0.0})
{
// it should be holding the given point
// i.e., active but trivial trajectory (one point only)
Expand Down

0 comments on commit e6e3374

Please sign in to comment.