From c3acc81c5b70407cc8933d20bb82c65c12a7fe7b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Jul 2023 14:10:37 +0000 Subject: [PATCH 01/17] Process tolerances sent with action goal --- .../joint_trajectory_controller.hpp | 3 + .../tolerances.hpp | 142 ++++++++- .../src/joint_trajectory_controller.cpp | 15 +- .../test/test_trajectory_actions.cpp | 284 +++++++++++++++++- .../test/test_trajectory_controller_utils.hpp | 8 +- 5 files changed, 438 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index dfbdf7436e..35d2c167d8 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -238,7 +238,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa size_t joint_names_size, const std::vector & vector_field, const std::string & string_for_vector_field, size_t i, bool allow_empty) const; + // the tolerances from the node parameter SegmentTolerances default_tolerances_; + // the tolerances used for the current goal + realtime_tools::RealtimeBuffer active_tolerances_; JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b4cb029dd9..326f84417c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -39,6 +39,7 @@ #include "joint_trajectory_controller_parameters.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" namespace joint_trajectory_controller { @@ -98,6 +99,8 @@ SegmentTolerances get_segment_tolerances(Params const & params) SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; + auto logger = rclcpp::get_logger("tolerance"); + RCLCPP_DEBUG(logger, "%s %f", "goal_time", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -109,16 +112,149 @@ SegmentTolerances get_segment_tolerances(Params const & params) tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal; tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance; - auto logger = rclcpp::get_logger("tolerance"); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position); + logger, "%s %f", (joint + ".trajectory.position").c_str(), + tolerances.state_tolerance[i].position); RCLCPP_DEBUG( - logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position); + logger, "%s %f", (joint + ".goal.position").c_str(), + tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal.velocity").c_str(), + tolerances.goal_state_tolerance[i].velocity); } return tolerances; } +/** + * \brief Populate trajectory segment tolerances using data from an action goal. + * + * \param default_tolerances The default tolerances to use if the action goal does not specify any. + * \param goal The new action goal + * \param params The ROS Parameters + * \return Trajectory segment tolerances. + */ +SegmentTolerances get_segment_tolerances( + SegmentTolerances const & default_tolerances, + control_msgs::action::FollowJointTrajectory::Goal const & goal, Params const & params) +{ + SegmentTolerances active_tolerances(default_tolerances); + + active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); + auto logger = rclcpp::get_logger("tolerance"); + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); + + // from + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg # + // There are two special values for tolerances: # * 0 - The tolerance is unspecified and will + // remain at whatever the default is # * -1 - The tolerance is "erased". If there was a default, + // the joint will be # allowed to move without restriction. + + // State and goal state tolerances + for (auto joint_tol : goal.path_tolerance) + { + auto const joint = joint_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(params.joints.begin(), params.joints.end(), joint); + if (it == params.joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint " + joint + "specified in goal.path_tolerance does not exist").c_str()); + return active_tolerances; + } + auto i = std::distance(params.joints.cbegin(), it); + if (joint_tol.position > 0.0) + { + active_tolerances.state_tolerance[i].position = joint_tol.position; + } + else if (joint_tol.position == -1.0) + { // -1 means erase + active_tolerances.state_tolerance[i].position = 0.0; + } + + if (joint_tol.velocity > 0.0) + { + active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; + } + else if (joint_tol.velocity == -1.0) + { // -1 means erase + active_tolerances.state_tolerance[i].velocity = 0.0; + } + + if (joint_tol.acceleration > 0.0) + { + active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; + } + else if (joint_tol.acceleration == -1.0) + { // -1 means erase + active_tolerances.state_tolerance[i].acceleration = 0.0; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.position").c_str(), + active_tolerances.state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(), + active_tolerances.state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(), + active_tolerances.state_tolerance[i].acceleration); + } + for (auto goal_tol : goal.goal_tolerance) + { + auto const joint = goal_tol.name; + // map joint names from goal to active_tolerances + auto it = std::find(params.joints.begin(), params.joints.end(), joint); + if (it == params.joints.end()) + { + RCLCPP_ERROR( + logger, "%s", + ("joint " + joint + "specified in goal.goal_tolerance does not exist").c_str()); + return active_tolerances; + } + auto i = std::distance(params.joints.cbegin(), it); + if (goal_tol.position > 0.0) + { + active_tolerances.goal_state_tolerance[i].position = goal_tol.position; + } + else if (goal_tol.position == -1.0) + { // -1 means erase + active_tolerances.goal_state_tolerance[i].position = 0.0; + } + + if (goal_tol.velocity > 0.0) + { + active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; + } + else if (goal_tol.velocity == -1.0) + { // -1 means erase + active_tolerances.goal_state_tolerance[i].velocity = 0.0; + } + + if (goal_tol.acceleration > 0.0) + { + active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; + } + else if (goal_tol.acceleration == -1.0) + { // -1 means erase + active_tolerances.goal_state_tolerance[i].acceleration = 0.0; + } + + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), + active_tolerances.goal_state_tolerance[i].position); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(), + active_tolerances.goal_state_tolerance[i].velocity); + RCLCPP_DEBUG( + logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(), + active_tolerances.goal_state_tolerance[i].acceleration); + } + + return active_tolerances; +} + /** * \param state_error State error to check. * \param joint_idx Joint index for the state error diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index dccb9fceff..e0c06ed8aa 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -213,6 +213,7 @@ controller_interface::return_type JointTrajectoryController::update( bool within_goal_time = true; double time_difference = 0.0; const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); + auto active_tol = active_tolerances_.readFromRT(); // Check state/goal tolerance for (size_t index = 0; index < dof_; ++index) @@ -224,7 +225,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, active_tol->state_tolerance[index], false)) { tolerance_violated_while_moving = true; } @@ -232,11 +233,11 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, active_tol->goal_state_tolerance[index], false)) { outside_goal_tolerance = true; - if (default_tolerances_.goal_time_tolerance != 0.0) + if (active_tol->goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time(); @@ -244,7 +245,7 @@ controller_interface::return_type JointTrajectoryController::update( time_difference = time.seconds() - traj_end.seconds(); - if (time_difference > default_tolerances_.goal_time_tolerance) + if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; } @@ -785,7 +786,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); default_tolerances_ = get_segment_tolerances(params_); - + active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); interpolation_method_ = interpolation_methods::from_string(interpolation_string); @@ -1142,6 +1143,10 @@ void JointTrajectoryController::goal_accepted_callback( rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); + // Update tolerances if specified in the goal + active_tolerances_.writeFromNonRT( + get_segment_tolerances(default_tolerances_, *(goal_handle->get_goal()), params_)); + // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fdea551c30..128277d360 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -153,10 +153,16 @@ class TestTrajectoryActions : public TrajectoryControllerTest using GoalOptions = rclcpp_action::Client::SendGoalOptions; std::shared_future sendActionGoal( - const std::vector & points, double timeout, const GoalOptions & opt) + const std::vector & points, double timeout, const GoalOptions & opt, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) { control_msgs::action::FollowJointTrajectory_Goal goal_msg; goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; goal_msg.trajectory.points = points; @@ -408,6 +414,276 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) } } +/** + * No need for parameterized tests + */ +TEST_F(TestTrajectoryActions, test_tolerances_via_actions) +{ + // set tolerance parameters + std::vector params = { + rclcpp::Parameter("constraints.joint1.goal", 0.1), + rclcpp::Parameter("constraints.joint2.goal", 0.1), + rclcpp::Parameter("constraints.joint3.goal", 0.1), + rclcpp::Parameter("constraints.goal_time", 0.1), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), + rclcpp::Parameter("constraints.joint1.trajectory", 0.1), + rclcpp::Parameter("constraints.joint2.trajectory", 0.1), + rclcpp::Parameter("constraints.joint3.trajectory", 0.1)}; + + SetUpExecutor(params); + + { + SetUpControllerHardware(); + std::shared_future gh_future; + // send goal + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances_ = traj_controller_->get_active_tolerances(); + + EXPECT_EQ(active_tolerances_.goal_time_tolerance, 1.0); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); + + EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + } + + // send goal with nonzero tolerances, are they accepted? + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 2.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances_ = traj_controller_->get_active_tolerances(); + EXPECT_EQ(active_tolerances_.goal_time_tolerance, 2.0); + + EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.2); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.4); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.2); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.4); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.2); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.4); + + EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 1.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 1.2); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 1.3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 3.3); + } + + // send goal without tolerances again, are the default ones used? + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, goal_options_); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances_ = traj_controller_->get_active_tolerances(); + + EXPECT_EQ(active_tolerances_.goal_time_tolerance, 1.0); + + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); + + EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.1); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + } + + // send goal with deactivated tolerances (-1) + { + SetUpControllerHardware(); + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances_ = traj_controller_->get_active_tolerances(); + EXPECT_EQ(active_tolerances_.goal_time_tolerance, 0.0); + + EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); + + EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + } +} + TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) { // set joint tolerance parameters @@ -619,7 +895,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); @@ -665,7 +942,8 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_tr TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_false) { std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false)}; + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", false), + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; SetUpExecutor(params); SetUpControllerHardware(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 5ec0f4f29e..4e788638d8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -123,6 +123,11 @@ class TestableJointTrajectoryController bool is_open_loop() { return params_.open_loop_control; } + joint_trajectory_controller::SegmentTolerances get_active_tolerances() + { + return *(active_tolerances_.readFromRT()); + } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -215,9 +220,6 @@ class TrajectoryControllerTest : public ::testing::Test { traj_controller_->get_node()->set_parameter(param); } - // ignore velocity tolerances for this test since they aren't committed in test_robot->write() - rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); - traj_controller_->get_node()->set_parameter(stopped_velocity_parameters); traj_controller_->get_node()->configure(); From 57d6b3264e4e76c71313b3d8f30d29daccb11f00 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 15 Aug 2023 22:38:56 +0200 Subject: [PATCH 02/17] Remove whitespaces --- .../test/test_trajectory_controller_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index e228a01d99..a6d3bde65a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -128,7 +128,7 @@ class TestableJointTrajectoryController { return *(active_tolerances_.readFromRT()); } - + bool has_active_traj() { return has_active_trajectory(); } bool has_trivial_traj() From 16a8828d9e5bd7c0e10518a41d9e5c5e49236a83 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 14 Sep 2023 16:53:26 +0000 Subject: [PATCH 03/17] Fix merge conflict --- .../src/joint_trajectory_controller.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 86065d951d..2d0263f887 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -265,11 +265,6 @@ controller_interface::return_type JointTrajectoryController::update( if (active_tol->goal_time_tolerance != 0.0) { // if we exceed goal_time_tolerance set it to aborted - const rclcpp::Time traj_start = traj_external_point_ptr_->get_trajectory_start_time(); - const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start; - - time_difference = time.seconds() - traj_end.seconds(); - if (time_difference > active_tol->goal_time_tolerance) { within_goal_time = false; From 534746cf27ed48a35585d6e25098167124c73788 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 22 Dec 2023 12:00:31 +0000 Subject: [PATCH 04/17] Fix format --- .../src/joint_trajectory_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a67394d70..256ed9c2e8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -261,7 +261,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */) && + state_error_, index, active_tol->goal_state_tolerance[index], + false /* show_errors */) && *(rt_is_holding_.readFromRT()) == false) { outside_goal_tolerance = true; From ae5efec280eb85a9cf03a6189d9b5dc479d22b40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 18 Jun 2024 11:32:43 +0200 Subject: [PATCH 05/17] Apply suggestions from code review Co-authored-by: Bence Magyar --- .../include/joint_trajectory_controller/tolerances.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 414e33f803..3732cce847 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -97,7 +97,7 @@ SegmentTolerances get_segment_tolerances(Params const & params) SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; auto logger = rclcpp::get_logger("tolerance"); - RCLCPP_DEBUG(logger, "%s %f", "goal_time", constraints.goal_time); + RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances tolerances.state_tolerance.resize(n_joints); @@ -143,9 +143,9 @@ SegmentTolerances get_segment_tolerances( // from // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg # - // There are two special values for tolerances: # * 0 - The tolerance is unspecified and will - // remain at whatever the default is # * -1 - The tolerance is "erased". If there was a default, - // the joint will be # allowed to move without restriction. + // There are two special values for tolerances: + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". If there was a default, the joint will be allowed to move without restriction. // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) From 597a2b890ad75bce68f14d04baceffecf692a2ea Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 18 Jun 2024 09:39:31 +0000 Subject: [PATCH 06/17] Use ERASE_CONSTANT --- .../tolerances.hpp | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 3732cce847..acdd2da86f 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -142,10 +142,12 @@ SegmentTolerances get_segment_tolerances( RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // from - // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg # + // https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg // There are two special values for tolerances: - // * 0 - The tolerance is unspecified and will remain at whatever the default is - // * -1 - The tolerance is "erased". If there was a default, the joint will be allowed to move without restriction. + // * 0 - The tolerance is unspecified and will remain at whatever the default is + // * -1 - The tolerance is "erased". + // If there was a default, the joint will be allowed to move without restriction. + constexpr double ERASE_VALUE = -1.0; // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -165,8 +167,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].position = joint_tol.position; } - else if (joint_tol.position == -1.0) - { // -1 means erase + else if (joint_tol.position == ERASE_VALUE) + { active_tolerances.state_tolerance[i].position = 0.0; } @@ -174,8 +176,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; } - else if (joint_tol.velocity == -1.0) - { // -1 means erase + else if (joint_tol.velocity == ERASE_VALUE) + { active_tolerances.state_tolerance[i].velocity = 0.0; } @@ -183,8 +185,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; } - else if (joint_tol.acceleration == -1.0) - { // -1 means erase + else if (joint_tol.acceleration == ERASE_VALUE) + { active_tolerances.state_tolerance[i].acceleration = 0.0; } @@ -215,8 +217,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].position = goal_tol.position; } - else if (goal_tol.position == -1.0) - { // -1 means erase + else if (goal_tol.position == ERASE_VALUE) + { active_tolerances.goal_state_tolerance[i].position = 0.0; } @@ -224,8 +226,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; } - else if (goal_tol.velocity == -1.0) - { // -1 means erase + else if (goal_tol.velocity == ERASE_VALUE) + { active_tolerances.goal_state_tolerance[i].velocity = 0.0; } @@ -233,8 +235,8 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; } - else if (goal_tol.acceleration == -1.0) - { // -1 means erase + else if (goal_tol.acceleration == ERASE_VALUE) + { active_tolerances.goal_state_tolerance[i].acceleration = 0.0; } From 88666ebd151badb1a197e1e994a22d7d1a6b7886 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 18 Jun 2024 09:45:17 +0000 Subject: [PATCH 07/17] Don't compare double with == -1 --- .../joint_trajectory_controller/tolerances.hpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index acdd2da86f..03d6511598 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -30,6 +30,7 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" @@ -148,6 +149,8 @@ SegmentTolerances get_segment_tolerances( // * -1 - The tolerance is "erased". // If there was a default, the joint will be allowed to move without restriction. constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -167,7 +170,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].position = joint_tol.position; } - else if (joint_tol.position == ERASE_VALUE) + else if (is_erase_value(joint_tol.position)) { active_tolerances.state_tolerance[i].position = 0.0; } @@ -176,7 +179,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; } - else if (joint_tol.velocity == ERASE_VALUE) + else if (is_erase_value(joint_tol.velocity)) { active_tolerances.state_tolerance[i].velocity = 0.0; } @@ -185,7 +188,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; } - else if (joint_tol.acceleration == ERASE_VALUE) + else if (is_erase_value(joint_tol.acceleration)) { active_tolerances.state_tolerance[i].acceleration = 0.0; } @@ -217,7 +220,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].position = goal_tol.position; } - else if (goal_tol.position == ERASE_VALUE) + else if (is_erase_value(goal_tol.position)) { active_tolerances.goal_state_tolerance[i].position = 0.0; } @@ -226,7 +229,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; } - else if (goal_tol.velocity == ERASE_VALUE) + else if (is_erase_value(goal_tol.velocity)) { active_tolerances.goal_state_tolerance[i].velocity = 0.0; } @@ -235,7 +238,7 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; } - else if (goal_tol.acceleration == ERASE_VALUE) + else if (is_erase_value(goal_tol.acceleration)) { active_tolerances.goal_state_tolerance[i].acceleration = 0.0; } From 7cc1244b36a791c5084e2b08ebea4f3abec09722 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 12:28:05 +0000 Subject: [PATCH 08/17] Don't accept negative tolerances --- .../tolerances.hpp | 58 ++- .../test/test_trajectory_actions.cpp | 409 +++++++++++++----- 2 files changed, 364 insertions(+), 103 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 03d6511598..6ea1a7a869 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -162,8 +162,8 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint " + joint + "specified in goal.path_tolerance does not exist").c_str()); - return active_tolerances; + ("joint '" + joint + "' specified in goal.path_tolerance does not exist").c_str()); + return default_tolerances; } auto i = std::distance(params.joints.cbegin(), it); if (joint_tol.position > 0.0) @@ -174,6 +174,14 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].position = 0.0; } + else if (joint_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + "' specified in goal.path_tolerance has a negative position tolerance") + .c_str()); + return default_tolerances; + } if (joint_tol.velocity > 0.0) { @@ -183,6 +191,14 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].velocity = 0.0; } + else if (joint_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + "' specified in goal.path_tolerance has a negative velocity tolerance") + .c_str()); + return default_tolerances; + } if (joint_tol.acceleration > 0.0) { @@ -192,6 +208,15 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.state_tolerance[i].acceleration = 0.0; } + else if (joint_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.path_tolerance has a negative acceleration tolerance") + .c_str()); + return default_tolerances; + } RCLCPP_DEBUG( logger, "%s %f", (joint + ".state_tolerance.position").c_str(), @@ -212,8 +237,8 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint " + joint + "specified in goal.goal_tolerance does not exist").c_str()); - return active_tolerances; + ("joint '" + joint + "' specified in goal.goal_tolerance does not exist").c_str()); + return default_tolerances; } auto i = std::distance(params.joints.cbegin(), it); if (goal_tol.position > 0.0) @@ -224,6 +249,14 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].position = 0.0; } + else if (goal_tol.position < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + "' specified in goal.goal_tolerance has a negative position tolerance") + .c_str()); + return default_tolerances; + } if (goal_tol.velocity > 0.0) { @@ -233,6 +266,14 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].velocity = 0.0; } + else if (goal_tol.velocity < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + "' specified in goal.goal_tolerance has a negative velocity tolerance") + .c_str()); + return default_tolerances; + } if (goal_tol.acceleration > 0.0) { @@ -242,6 +283,15 @@ SegmentTolerances get_segment_tolerances( { active_tolerances.goal_state_tolerance[i].acceleration = 0.0; } + else if (goal_tol.acceleration < 0.0) + { + RCLCPP_ERROR( + logger, "%s", + ("joint '" + joint + + "' specified in goal.goal_tolerance has a negative acceleration tolerance") + .c_str()); + return default_tolerances; + } RCLCPP_DEBUG( logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(), diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 33fa724737..63fdfe692f 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -494,6 +494,33 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } +void check_default_tolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + EXPECT_EQ(active_tolerances.goal_time_tolerance, 1.0); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + /** * No need for parameterized tests */ @@ -536,31 +563,8 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - auto active_tolerances_ = traj_controller_->get_active_tolerances(); - - EXPECT_EQ(active_tolerances_.goal_time_tolerance, 1.0); - // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance - EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); - - EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); } // send goal with nonzero tolerances, are they accepted? @@ -617,30 +621,30 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - auto active_tolerances_ = traj_controller_->get_active_tolerances(); - EXPECT_EQ(active_tolerances_.goal_time_tolerance, 2.0); - - EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.2); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.4); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.2); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.4); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.2); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.4); - - EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 1.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 2.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 3.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 1.2); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 2.2); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 3.2); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 1.3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 2.3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 3.3); + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_EQ(active_tolerances.goal_time_tolerance, 2.0); + + EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); } // send goal without tolerances again, are the default ones used? @@ -668,32 +672,8 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - auto active_tolerances_ = traj_controller_->get_active_tolerances(); - - EXPECT_EQ(active_tolerances_.goal_time_tolerance, 1.0); - - // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance - EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); - - EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.1); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); } // send goal with deactivated tolerances (-1) @@ -737,30 +717,261 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) EXPECT_EQ( control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - auto active_tolerances_ = traj_controller_->get_active_tolerances(); - EXPECT_EQ(active_tolerances_.goal_time_tolerance, 0.0); - - EXPECT_EQ(active_tolerances_.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).position, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).position, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).position, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances_.state_tolerance.at(2).acceleration, 0.0); - - EXPECT_EQ(active_tolerances_.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).position, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).position, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).position, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances_.goal_state_tolerance.at(2).acceleration, 0.0); + auto active_tolerances = traj_controller_->get_active_tolerances(); + + EXPECT_EQ(active_tolerances.goal_time_tolerance, 0.0); + + EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); + } + + // send goal with invalid tolerances, are the default ones used? + // negative path position tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); + } + // negative path velocity tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); + } + // negative path acceleration tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); + } + // negative goal position tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); + } + // negative goal velocity tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); + } + // negative goal acceleration tolerance + { + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + check_default_tolerances(active_tolerances); } } From 6d21d3a4973cffb771de5627303b54c1b46133cc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 13:11:26 +0000 Subject: [PATCH 09/17] Fix tests and improve error messages. --- .../tolerances.hpp | 34 +++- .../test/test_trajectory_actions.cpp | 159 ++++++++---------- .../test/test_trajectory_controller_utils.hpp | 26 +++ 3 files changed, 122 insertions(+), 97 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 6ea1a7a869..b915a38125 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -147,7 +147,7 @@ SegmentTolerances get_segment_tolerances( // There are two special values for tolerances: // * 0 - The tolerance is unspecified and will remain at whatever the default is // * -1 - The tolerance is "erased". - // If there was a default, the joint will be allowed to move without restriction. + // If there was a default, the joint will be allowed to move without restriction. constexpr double ERASE_VALUE = -1.0; auto is_erase_value = [](double value) { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; @@ -162,7 +162,10 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.path_tolerance does not exist").c_str()); + ("joint '" + joint + + "' specified in goal.path_tolerance does not exist. " + "Use default tolerances.") + .c_str()); return default_tolerances; } auto i = std::distance(params.joints.cbegin(), it); @@ -178,7 +181,9 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.path_tolerance has a negative position tolerance") + ("joint '" + joint + + "' specified in goal.path_tolerance has a negative position tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } @@ -195,7 +200,9 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.path_tolerance has a negative velocity tolerance") + ("joint '" + joint + + "' specified in goal.path_tolerance has a negative velocity tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } @@ -213,7 +220,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.path_tolerance has a negative acceleration tolerance") + "' specified in goal.path_tolerance has a negative acceleration tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } @@ -237,7 +245,10 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.goal_tolerance does not exist").c_str()); + ("joint '" + joint + + "' specified in goal.goal_tolerance does not exist. " + "Use default tolerances.") + .c_str()); return default_tolerances; } auto i = std::distance(params.joints.cbegin(), it); @@ -253,7 +264,9 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.goal_tolerance has a negative position tolerance") + ("joint '" + joint + + "' specified in goal.goal_tolerance has a negative position tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } @@ -270,7 +283,9 @@ SegmentTolerances get_segment_tolerances( { RCLCPP_ERROR( logger, "%s", - ("joint '" + joint + "' specified in goal.goal_tolerance has a negative velocity tolerance") + ("joint '" + joint + + "' specified in goal.goal_tolerance has a negative velocity tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } @@ -288,7 +303,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.goal_tolerance has a negative acceleration tolerance") + "' specified in goal.goal_tolerance has a negative acceleration tolerance. " + "Use default tolerances.") .c_str()); return default_tolerances; } diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 63fdfe692f..fa1bee531c 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -494,44 +494,18 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) expectCommandPoint(points_positions.at(1)); } -void check_default_tolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) -{ - EXPECT_EQ(active_tolerances.goal_time_tolerance, 1.0); - // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance - EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); - - EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); -} - /** * No need for parameterized tests */ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) { // set tolerance parameters + constexpr double default_goal_time = 0.1; std::vector params = { rclcpp::Parameter("constraints.joint1.goal", 0.1), rclcpp::Parameter("constraints.joint2.goal", 0.1), rclcpp::Parameter("constraints.joint3.goal", 0.1), - rclcpp::Parameter("constraints.goal_time", 0.1), + rclcpp::Parameter("constraints.goal_time", default_goal_time), rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.1), rclcpp::Parameter("constraints.joint1.trajectory", 0.1), rclcpp::Parameter("constraints.joint2.trajectory", 0.1), @@ -540,6 +514,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) SetUpExecutor(params); { + SCOPED_TRACE("Check default values"); SetUpControllerHardware(); std::shared_future gh_future; // send goal @@ -564,7 +539,8 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); } // send goal with nonzero tolerances, are they accepted? @@ -622,29 +598,29 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_EQ(active_tolerances.goal_time_tolerance, 2.0); - - EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); - - EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); } // send goal without tolerances again, are the default ones used? @@ -673,7 +649,8 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); + expectDefaultTolerances(active_tolerances); } // send goal with deactivated tolerances (-1) @@ -719,34 +696,34 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_EQ(active_tolerances.goal_time_tolerance, 0.0); - - EXPECT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); - - EXPECT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); - EXPECT_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); } // send goal with invalid tolerances, are the default ones used? - // negative path position tolerance { + SCOPED_TRACE("negative path position tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -781,10 +758,11 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } - // negative path velocity tolerance { + SCOPED_TRACE("negative path velocity tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -819,10 +797,11 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } - // negative path acceleration tolerance { + SCOPED_TRACE("negative path acceleration tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -857,10 +836,11 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } - // negative goal position tolerance { + SCOPED_TRACE("negative goal position tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -895,10 +875,11 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } - // negative goal velocity tolerance { + SCOPED_TRACE("negative goal velocity tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -933,10 +914,11 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } - // negative goal acceleration tolerance { + SCOPED_TRACE("negative goal acceleration tolerance"); SetUpControllerHardware(); std::shared_future gh_future; @@ -971,7 +953,8 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); auto active_tolerances = traj_controller_->get_active_tolerances(); - check_default_tolerances(active_tolerances); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 01cde7e68d..649e173d0e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -651,6 +651,32 @@ class TrajectoryControllerTest : public ::testing::Test } } + void expectDefaultTolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) + { + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); + } + /** * @brief compares the joint names and interface types of the controller with the given ones */ From a5d548cee50e7300199191d779d1f12864ca179b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 13:29:07 +0000 Subject: [PATCH 10/17] Add a test for wrong joints too --- .../test/test_trajectory_actions.cpp | 100 ++++++++++++++---- 1 file changed, 82 insertions(+), 18 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fa1bee531c..9fa1567764 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -739,16 +739,14 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) points.push_back(point); std::vector path_tolerance; - std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = -123.0; tolerance.velocity = 0.0; tolerance.acceleration = 0.0; path_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); } controller_hw_thread_.join(); @@ -778,16 +776,14 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) points.push_back(point); std::vector path_tolerance; - std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = 0.0; tolerance.velocity = -123.0; tolerance.acceleration = 0.0; path_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); } controller_hw_thread_.join(); @@ -817,16 +813,14 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) points.push_back(point); std::vector path_tolerance; - std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = 0.0; tolerance.velocity = 0.0; tolerance.acceleration = -123.0; path_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); } controller_hw_thread_.join(); @@ -855,17 +849,17 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - std::vector path_tolerance; std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = -123.0; tolerance.velocity = 0.0; tolerance.acceleration = 0.0; goal_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal( + points, 0.0, goal_options_, std::vector(), + goal_tolerance); } controller_hw_thread_.join(); @@ -894,17 +888,17 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - std::vector path_tolerance; std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = 0.0; tolerance.velocity = -123.0; tolerance.acceleration = 0.0; goal_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal( + points, 0.0, goal_options_, std::vector(), + goal_tolerance); } controller_hw_thread_.join(); @@ -933,17 +927,87 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - std::vector path_tolerance; std::vector goal_tolerance; control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order tolerance.name = "joint1"; tolerance.position = 0.0; tolerance.velocity = 0.0; tolerance.acceleration = -123.0; goal_tolerance.push_back(tolerance); - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); + gh_future = sendActionGoal( + points, 0.0, goal_options_, std::vector(), + goal_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("unknown joint in path acceleration tolerance"); + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); + } + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + EXPECT_EQ( + control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); + + auto active_tolerances = traj_controller_->get_active_tolerances(); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("unknown joint in goal position tolerance"); + SetUpControllerHardware(); + + std::shared_future gh_future; + { + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + gh_future = sendActionGoal( + points, 0.0, goal_options_, std::vector(), + goal_tolerance); } controller_hw_thread_.join(); From 4f1b75592041a2d8b660b49cdf37663dcbcedd65 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 20 Jun 2024 14:42:05 +0000 Subject: [PATCH 11/17] Move tolerances tests to own file --- joint_trajectory_controller/CMakeLists.txt | 4 + .../test/test_tolerances.cpp | 422 ++++++++++++++++++ .../test/test_tolerances_utils.hpp | 57 +++ .../test/test_trajectory_actions.cpp | 375 +--------------- .../test/test_trajectory_controller_utils.hpp | 27 -- 5 files changed, 487 insertions(+), 398 deletions(-) create mode 100644 joint_trajectory_controller/test/test_tolerances.cpp create mode 100644 joint_trajectory_controller/test/test_tolerances_utils.hpp diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index 4b0ca82df8..46c245be9c 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -61,6 +61,10 @@ if(BUILD_TESTING) target_link_libraries(test_trajectory joint_trajectory_controller) target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_tolerances test/test_tolerances.cpp) + target_link_libraries(test_tolerances joint_trajectory_controller) + target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES) + ament_add_gmock(test_trajectory_controller test/test_trajectory_controller.cpp) set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp new file mode 100644 index 0000000000..1e922372fb --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -0,0 +1,422 @@ +// Copyright 2024 Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "gmock/gmock.h" +#include "rclcpp/duration.hpp" + +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_tolerances_utils.hpp" +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" + +using joint_trajectory_controller::SegmentTolerances; +using trajectory_msgs::msg::JointTrajectoryPoint; + +std::vector joint_names_ = {"joint1", "joint2", "joint3"}; + +control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg( + const std::vector & points, double timeout, + const std::vector path_tolerance = + std::vector(), + const std::vector goal_tolerance = + std::vector()) +{ + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(timeout); + goal_msg.goal_tolerance = goal_tolerance; + goal_msg.path_tolerance = path_tolerance; + goal_msg.trajectory.joint_names = joint_names_; + goal_msg.trajectory.points = points; + + return goal_msg; +} +class TestTolerancesFixture : public ::testing::Test +{ +protected: + SegmentTolerances default_tolerances; + joint_trajectory_controller::Params params; + std::vector joint_names_; + + void SetUp() override + { + // Initialize joint_names_ with some test data + joint_names_ = {"joint1", "joint2", "joint3"}; + + // Initialize default_tolerances and params with common setup for all tests + // TODO(anyone) fill params and use + // SegmentTolerances get_segment_tolerances(Params const & params) instead + default_tolerances.goal_time_tolerance = default_goal_time; + default_tolerances.state_tolerance.resize(joint_names_.size()); + default_tolerances.goal_state_tolerance.resize(joint_names_.size()); + for (size_t i = 0; i < joint_names_.size(); ++i) + { + default_tolerances.state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).position = 0.1; + default_tolerances.goal_state_tolerance.at(i).velocity = stopped_velocity_tolerance; + } + params.joints = joint_names_; + } + + void TearDown() override + { + // Cleanup code if necessary + } +}; + +TEST_F(TestTolerancesFixture, test_get_segment_tolerances) +{ + // send goal with nonzero tolerances, are they accepted? + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = 0.2; + tolerance.velocity = 0.3; + tolerance.acceleration = 0.4; + path_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + std::vector goal_tolerance; + // add different tolerances in jumbled order + tolerance.name = "joint2"; + tolerance.position = 1.2; + tolerance.velocity = 2.2; + tolerance.acceleration = 3.2; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + tolerance.position = 1.3; + tolerance.velocity = 2.3; + tolerance.acceleration = 3.3; + goal_tolerance.push_back(tolerance); + tolerance.name = "joint1"; + tolerance.position = 1.1; + tolerance.velocity = 2.1; + tolerance.acceleration = 3.1; + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.4); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.2); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.4); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 1.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 2.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 3.1); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 1.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 2.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 3.2); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 1.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 2.3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 3.3); +} + +// send goal with deactivated tolerances (-1) +TEST_F(TestTolerancesFixture, test_deactivate_tolerances) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + // add the same tolerance for every joint, give it in correct order + tolerance.name = "joint1"; + tolerance.position = -1.0; + tolerance.velocity = -1.0; + tolerance.acceleration = -1.0; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint2"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + tolerance.name = "joint3"; + path_tolerance.push_back(tolerance); + goal_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +// send goal with invalid tolerances, are the default ones used? +TEST_F(TestTolerancesFixture, test_invalid_tolerances) +{ + { + SCOPED_TRACE("negative path position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative path acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal position tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = -123.0; + tolerance.velocity = 0.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal velocity tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = -123.0; + tolerance.acceleration = 0.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } + { + SCOPED_TRACE("negative goal acceleration tolerance"); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint1"; + tolerance.position = 0.0; + tolerance.velocity = 0.0; + tolerance.acceleration = -123.0; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); + } +} +TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector path_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} +TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) +{ + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions.resize(joint_names_.size()); + + point.positions[0] = 1.0; + point.positions[1] = 2.0; + point.positions[2] = 3.0; + points.push_back(point); + + std::vector goal_tolerance; + control_msgs::msg::JointTolerance tolerance; + tolerance.name = "joint123"; + goal_tolerance.push_back(tolerance); + + auto goal_msg = + prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); + auto active_tolerances = + joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + expectDefaultTolerances(active_tolerances); +} diff --git a/joint_trajectory_controller/test/test_tolerances_utils.hpp b/joint_trajectory_controller/test/test_tolerances_utils.hpp new file mode 100644 index 0000000000..622656aa99 --- /dev/null +++ b/joint_trajectory_controller/test/test_tolerances_utils.hpp @@ -0,0 +1,57 @@ +// Copyright 2024 Austrian Institute of Technology +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_TOLERANCES_UTILS_HPP_ +#define TEST_TOLERANCES_UTILS_HPP_ + +#include + +#include "gmock/gmock.h" + +#include "joint_trajectory_controller/tolerances.hpp" + +double default_goal_time = 0.1; +double stopped_velocity_tolerance = 0.1; + +void expectDefaultTolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + +#endif // TEST_TOLERANCES_UTILS_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 9fa1567764..c626326cc8 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -31,7 +31,6 @@ #include "controller_interface/controller_interface.hpp" #include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -44,10 +43,13 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_tolerances_utils.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; @@ -500,7 +502,6 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) TEST_F(TestTrajectoryActions, test_tolerances_via_actions) { // set tolerance parameters - constexpr double default_goal_time = 0.1; std::vector params = { rclcpp::Parameter("constraints.joint1.goal", 0.1), rclcpp::Parameter("constraints.joint2.goal", 0.1), @@ -652,374 +653,6 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } - - // send goal with deactivated tolerances (-1) - { - SetUpControllerHardware(); - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector path_tolerance; - std::vector goal_tolerance; - control_msgs::msg::JointTolerance tolerance; - // add the same tolerance for every joint, give it in correct order - tolerance.name = "joint1"; - tolerance.position = -1.0; - tolerance.velocity = -1.0; - tolerance.acceleration = -1.0; - path_tolerance.push_back(tolerance); - goal_tolerance.push_back(tolerance); - tolerance.name = "joint2"; - path_tolerance.push_back(tolerance); - goal_tolerance.push_back(tolerance); - tolerance.name = "joint3"; - path_tolerance.push_back(tolerance); - goal_tolerance.push_back(tolerance); - - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance, goal_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); - - ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); - - ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); - } - - // send goal with invalid tolerances, are the default ones used? - { - SCOPED_TRACE("negative path position tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector path_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = -123.0; - tolerance.velocity = 0.0; - tolerance.acceleration = 0.0; - path_tolerance.push_back(tolerance); - - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("negative path velocity tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector path_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = 0.0; - tolerance.velocity = -123.0; - tolerance.acceleration = 0.0; - path_tolerance.push_back(tolerance); - - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("negative path acceleration tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector path_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = 0.0; - tolerance.velocity = 0.0; - tolerance.acceleration = -123.0; - path_tolerance.push_back(tolerance); - - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("negative goal position tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector goal_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = -123.0; - tolerance.velocity = 0.0; - tolerance.acceleration = 0.0; - goal_tolerance.push_back(tolerance); - - gh_future = sendActionGoal( - points, 0.0, goal_options_, std::vector(), - goal_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("negative goal velocity tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector goal_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = 0.0; - tolerance.velocity = -123.0; - tolerance.acceleration = 0.0; - goal_tolerance.push_back(tolerance); - - gh_future = sendActionGoal( - points, 0.0, goal_options_, std::vector(), - goal_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("negative goal acceleration tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector goal_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint1"; - tolerance.position = 0.0; - tolerance.velocity = 0.0; - tolerance.acceleration = -123.0; - goal_tolerance.push_back(tolerance); - - gh_future = sendActionGoal( - points, 0.0, goal_options_, std::vector(), - goal_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("unknown joint in path acceleration tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector path_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint123"; - path_tolerance.push_back(tolerance); - - gh_future = sendActionGoal(points, 0.0, goal_options_, path_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } - { - SCOPED_TRACE("unknown joint in goal position tolerance"); - SetUpControllerHardware(); - - std::shared_future gh_future; - { - std::vector points; - JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - - point.positions[0] = 1.0; - point.positions[1] = 2.0; - point.positions[2] = 3.0; - points.push_back(point); - - std::vector goal_tolerance; - control_msgs::msg::JointTolerance tolerance; - tolerance.name = "joint123"; - goal_tolerance.push_back(tolerance); - - gh_future = sendActionGoal( - points, 0.0, goal_options_, std::vector(), - goal_tolerance); - } - controller_hw_thread_.join(); - - EXPECT_TRUE(gh_future.get()); - EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); - EXPECT_EQ( - control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL, common_action_result_code_); - - auto active_tolerances = traj_controller_->get_active_tolerances(); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); - expectDefaultTolerances(active_tolerances); - } } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 649e173d0e..f0ccacee5b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -42,7 +42,6 @@ bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); } - } // namespace namespace test_trajectory_controllers @@ -651,32 +650,6 @@ class TrajectoryControllerTest : public ::testing::Test } } - void expectDefaultTolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) - { - // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance - ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); - - ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).velocity, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).velocity, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).velocity, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); - } - /** * @brief compares the joint names and interface types of the controller with the given ones */ From ec87dc04d3964a1c38020a2d3ab64b84bd053a72 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Fri, 21 Jun 2024 07:40:32 +0200 Subject: [PATCH 12/17] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../tolerances.hpp | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index b915a38125..8d7a7e4444 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -133,8 +133,8 @@ SegmentTolerances get_segment_tolerances(Params const & params) * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - SegmentTolerances const & default_tolerances, - control_msgs::action::FollowJointTrajectory::Goal const & goal, Params const & params) + const SegmentTolerances & default_tolerances, + const control_msgs::action::FollowJointTrajectory::Goal & goal, const Params & params) { SegmentTolerances active_tolerances(default_tolerances); @@ -164,7 +164,7 @@ SegmentTolerances get_segment_tolerances( logger, "%s", ("joint '" + joint + "' specified in goal.path_tolerance does not exist. " - "Use default tolerances.") + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -182,8 +182,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.path_tolerance has a negative position tolerance. " - "Use default tolerances.") + "' specified in goal.path_tolerance has a invalid position tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -201,8 +201,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.path_tolerance has a negative velocity tolerance. " - "Use default tolerances.") + "' specified in goal.path_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -220,8 +220,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.path_tolerance has a negative acceleration tolerance. " - "Use default tolerances.") + "' specified in goal.path_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -247,7 +247,7 @@ SegmentTolerances get_segment_tolerances( logger, "%s", ("joint '" + joint + "' specified in goal.goal_tolerance does not exist. " - "Use default tolerances.") + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -265,8 +265,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.goal_tolerance has a negative position tolerance. " - "Use default tolerances.") + "' specified in goal.goal_tolerance has a invalid position tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -284,8 +284,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.goal_tolerance has a negative velocity tolerance. " - "Use default tolerances.") + "' specified in goal.goal_tolerance has a invalid velocity tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } @@ -303,8 +303,8 @@ SegmentTolerances get_segment_tolerances( RCLCPP_ERROR( logger, "%s", ("joint '" + joint + - "' specified in goal.goal_tolerance has a negative acceleration tolerance. " - "Use default tolerances.") + "' specified in goal.goal_tolerance has a invalid acceleration tolerance. " + "Using default tolerances.") .c_str()); return default_tolerances; } From 54d279cde54e425117a965c2429fccef49810247 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 22 Jun 2024 10:12:16 +0000 Subject: [PATCH 13/17] Remove duplicate check --- .../src/joint_trajectory_controller.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 2f34fdf33d..76e6387817 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -225,8 +225,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, active_tol->state_tolerance[index], true /* show_errors */) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, active_tol->state_tolerance[index], true /* show_errors */)) { tolerance_violated_while_moving = true; } @@ -234,9 +233,7 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && *(rt_is_holding_.readFromRT()) == false && !check_state_tolerance_per_joint( - state_error_, index, active_tol->goal_state_tolerance[index], - false /* show_errors */) && - *(rt_is_holding_.readFromRT()) == false) + state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) { outside_goal_tolerance = true; From 1a592abdf9da030cc998bf8cb0b426f7441fd19f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Jun 2024 19:25:18 +0000 Subject: [PATCH 14/17] Pass std::vector instead of parameter struct --- .../tolerances.hpp | 20 +++++----- .../src/joint_trajectory_controller.cpp | 2 +- .../test/test_tolerances.cpp | 40 +++++++++---------- 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 8d7a7e4444..742be12baf 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -31,6 +31,7 @@ #define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_ #include +#include #include #include "control_msgs/action/follow_joint_trajectory.hpp" @@ -90,7 +91,7 @@ struct SegmentTolerances * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(Params const & params) +SegmentTolerances get_segment_tolerances(const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); @@ -129,12 +130,13 @@ SegmentTolerances get_segment_tolerances(Params const & params) * * \param default_tolerances The default tolerances to use if the action goal does not specify any. * \param goal The new action goal - * \param params The ROS Parameters + * \param joints The joints configured by ROS parameters * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( const SegmentTolerances & default_tolerances, - const control_msgs::action::FollowJointTrajectory::Goal & goal, const Params & params) + const control_msgs::action::FollowJointTrajectory::Goal & goal, + const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); @@ -157,8 +159,8 @@ SegmentTolerances get_segment_tolerances( { auto const joint = joint_tol.name; // map joint names from goal to active_tolerances - auto it = std::find(params.joints.begin(), params.joints.end(), joint); - if (it == params.joints.end()) + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) { RCLCPP_ERROR( logger, "%s", @@ -168,7 +170,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(params.joints.cbegin(), it); + auto i = std::distance(joints.cbegin(), it); if (joint_tol.position > 0.0) { active_tolerances.state_tolerance[i].position = joint_tol.position; @@ -240,8 +242,8 @@ SegmentTolerances get_segment_tolerances( { auto const joint = goal_tol.name; // map joint names from goal to active_tolerances - auto it = std::find(params.joints.begin(), params.joints.end(), joint); - if (it == params.joints.end()) + auto it = std::find(joints.begin(), joints.end(), joint); + if (it == joints.end()) { RCLCPP_ERROR( logger, "%s", @@ -251,7 +253,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(params.joints.cbegin(), it); + auto i = std::distance(joints.cbegin(), it); if (goal_tol.position > 0.0) { active_tolerances.goal_state_tolerance[i].position = goal_tol.position; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 76e6387817..16be5a1f3f 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1177,7 +1177,7 @@ void JointTrajectoryController::goal_accepted_callback( // Update tolerances if specified in the goal active_tolerances_.writeFromNonRT( - get_segment_tolerances(default_tolerances_, *(goal_handle->get_goal()), params_)); + get_segment_tolerances(default_tolerances_, *(goal_handle->get_goal()), params_.joints)); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 1e922372fb..70fbd5b662 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -123,8 +123,8 @@ TEST_F(TestTolerancesFixture, test_get_segment_tolerances) goal_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); @@ -182,8 +182,8 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) goal_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); @@ -234,8 +234,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) path_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -260,8 +260,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) path_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -286,8 +286,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) path_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -313,8 +313,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -340,8 +340,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -367,8 +367,8 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -391,8 +391,8 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) path_tolerance.push_back(tolerance); auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -415,8 +415,8 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); - auto active_tolerances = - joint_trajectory_controller::get_segment_tolerances(default_tolerances, goal_msg, params); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } From 2bdc0c2fd553b16950a2704931d75be85bca065c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Jun 2024 20:21:39 +0000 Subject: [PATCH 15/17] Use a static child logger --- .../tolerances.hpp | 10 +++-- .../src/joint_trajectory_controller.cpp | 43 ++++++++++--------- .../test/test_tolerances.cpp | 22 +++++----- 3 files changed, 41 insertions(+), 34 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 742be12baf..1998930182 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -88,17 +88,18 @@ struct SegmentTolerances * goal: 0.01 * \endcode * + * \param jtc_logger The logger to use for output * \param params The ROS Parameters * \return Trajectory segment tolerances. */ -SegmentTolerances get_segment_tolerances(const Params & params) +SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params) { auto const & constraints = params.constraints; auto const n_joints = params.joints.size(); SegmentTolerances tolerances; tolerances.goal_time_tolerance = constraints.goal_time; - auto logger = rclcpp::get_logger("tolerance"); + static auto logger = jtc_logger.get_child("tolerance"); RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time); // State and goal state tolerances @@ -128,20 +129,21 @@ SegmentTolerances get_segment_tolerances(const Params & params) /** * \brief Populate trajectory segment tolerances using data from an action goal. * + * \param jtc_logger The logger to use for output * \param default_tolerances The default tolerances to use if the action goal does not specify any. * \param goal The new action goal * \param joints The joints configured by ROS parameters * \return Trajectory segment tolerances. */ SegmentTolerances get_segment_tolerances( - const SegmentTolerances & default_tolerances, + rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances, const control_msgs::action::FollowJointTrajectory::Goal & goal, const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); - auto logger = rclcpp::get_logger("tolerance"); + static auto logger = jtc_logger.get_child("tolerance"); RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // from diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 16be5a1f3f..31598bb813 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -126,11 +126,12 @@ controller_interface::return_type JointTrajectoryController::update( { return controller_interface::return_type::OK; } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) { params_ = param_listener_->get_params(); - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // update the PID gains // variable use_closed_loop_pid_adapter_ is updated in on_configure only if (use_closed_loop_pid_adapter_) @@ -208,7 +209,7 @@ controller_interface::return_type JointTrajectoryController::update( !before_last_point && *(rt_is_holding_.readFromRT()) == false && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) { - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to command timeout"); + RCLCPP_WARN(logger, "Aborted due to command timeout"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -320,7 +321,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), "Aborted due to state tolerance violation"); + RCLCPP_WARN(logger, "Aborted due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -339,7 +340,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); + RCLCPP_INFO(logger, "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); @@ -358,7 +359,7 @@ controller_interface::return_type JointTrajectoryController::update( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); rt_has_pending_goal_.writeFromNonRT(false); - RCLCPP_WARN(get_node()->get_logger(), error_string.c_str()); + RCLCPP_WARN(logger, error_string.c_str()); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -368,7 +369,7 @@ controller_interface::return_type JointTrajectoryController::update( else if (tolerance_violated_while_moving && *(rt_has_pending_goal_.readFromRT()) == false) { // we need to ensure that there is no pending goal -> we get a race condition otherwise - RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); + RCLCPP_ERROR(logger, "Holding position due to state tolerance violation"); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -376,7 +377,7 @@ controller_interface::return_type JointTrajectoryController::update( else if ( !before_last_point && !within_goal_time && *(rt_has_pending_goal_.readFromRT()) == false) { - RCLCPP_ERROR(get_node()->get_logger(), "Exceeded goal_time_tolerance: holding position..."); + RCLCPP_ERROR(logger, "Exceeded goal_time_tolerance: holding position..."); traj_msg_external_point_ptr_.reset(); traj_msg_external_point_ptr_.initRT(set_hold_position()); @@ -620,11 +621,11 @@ void JointTrajectoryController::query_state_service( controller_interface::CallbackReturn JointTrajectoryController::on_configure( const rclcpp_lifecycle::State &) { - const auto logger = get_node()->get_logger(); + auto logger = get_node()->get_logger(); if (!param_listener_) { - RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + RCLCPP_ERROR(logger, "Error encountered during init"); return controller_interface::CallbackReturn::ERROR; } @@ -782,7 +783,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( get_interface_list(params_.state_interfaces).c_str()); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); active_tolerances_.initRT(default_tolerances_); const std::string interpolation_string = get_node()->get_parameter("interpolation_method").as_string(); @@ -875,6 +876,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + auto logger = get_node()->get_logger(); + // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); @@ -882,7 +885,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( params_ = param_listener_->get_params(); // parse remaining parameters - default_tolerances_ = get_segment_tolerances(params_); + default_tolerances_ = get_segment_tolerances(logger, params_); // order all joints in the storage for (const auto & interface : params_.command_interfaces) @@ -894,8 +897,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, - interface.c_str(), joint_command_interface_[index].size()); + logger, "Expected %zu '%s' command interfaces, got %zu.", dof_, interface.c_str(), + joint_command_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -908,8 +911,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' state interfaces, got %zu.", dof_, - interface.c_str(), joint_state_interface_[index].size()); + logger, "Expected %zu '%s' state interfaces, got %zu.", dof_, interface.c_str(), + joint_state_interface_[index].size()); return CallbackReturn::ERROR; } } @@ -951,9 +954,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // deactivate timeout RCLCPP_WARN( - get_node()->get_logger(), - "Command timeout must be higher than goal_time tolerance (%f vs. %f)", params_.cmd_timeout, - default_tolerances_.goal_time_tolerance); + logger, "Command timeout must be higher than goal_time tolerance (%f vs. %f)", + params_.cmd_timeout, default_tolerances_.goal_time_tolerance); cmd_timeout_ = 0.0; } } @@ -1176,8 +1178,9 @@ void JointTrajectoryController::goal_accepted_callback( rt_active_goal_.writeFromNonRT(rt_goal); // Update tolerances if specified in the goal - active_tolerances_.writeFromNonRT( - get_segment_tolerances(default_tolerances_, *(goal_handle->get_goal()), params_.joints)); + auto logger = this->get_node()->get_logger(); + active_tolerances_.writeFromNonRT(get_segment_tolerances( + logger, default_tolerances_, *(goal_handle->get_goal()), params_.joints)); // Set smartpointer to expire for create_wall_timer to delete previous entry from timer list goal_handle_timer_.reset(); diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 70fbd5b662..fd51ca61bc 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -19,6 +19,7 @@ #include "gmock/gmock.h" #include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" #include "joint_trajectory_controller/tolerances.hpp" #include "test_tolerances_utils.hpp" @@ -52,6 +53,7 @@ class TestTolerancesFixture : public ::testing::Test SegmentTolerances default_tolerances; joint_trajectory_controller::Params params; std::vector joint_names_; + rclcpp::Logger logger = rclcpp::get_logger("TestTolerancesFixture"); void SetUp() override { @@ -124,7 +126,7 @@ TEST_F(TestTolerancesFixture, test_get_segment_tolerances) auto goal_msg = prepareGoalMsg(points, 2.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 2.0); @@ -183,7 +185,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 0.0); @@ -235,7 +237,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -261,7 +263,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -287,7 +289,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -314,7 +316,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -341,7 +343,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -368,7 +370,7 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -392,7 +394,7 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_path_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } @@ -416,7 +418,7 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) auto goal_msg = prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( - default_tolerances, goal_msg, params.joints); + logger, default_tolerances, goal_msg, params.joints); EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } From 36ecdda94fd2dbdbae37c48ff18d35b66123389f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Jun 2024 20:31:59 +0000 Subject: [PATCH 16/17] Remove test_tolerances_utils --- .../test/test_tolerances.cpp | 6 +- .../test/test_tolerances_utils.hpp | 57 ------------------- .../test/test_trajectory_actions.cpp | 1 - .../test/test_trajectory_controller_utils.hpp | 35 ++++++++++++ 4 files changed, 38 insertions(+), 61 deletions(-) delete mode 100644 joint_trajectory_controller/test/test_tolerances_utils.hpp diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index fd51ca61bc..66914b6da4 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -20,12 +20,12 @@ #include "gmock/gmock.h" #include "rclcpp/duration.hpp" #include "rclcpp/logger.hpp" - -#include "joint_trajectory_controller/tolerances.hpp" -#include "test_tolerances_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/tolerances.hpp" +#include "test_trajectory_controller_utils.hpp" + using joint_trajectory_controller::SegmentTolerances; using trajectory_msgs::msg::JointTrajectoryPoint; diff --git a/joint_trajectory_controller/test/test_tolerances_utils.hpp b/joint_trajectory_controller/test/test_tolerances_utils.hpp deleted file mode 100644 index 622656aa99..0000000000 --- a/joint_trajectory_controller/test/test_tolerances_utils.hpp +++ /dev/null @@ -1,57 +0,0 @@ -// Copyright 2024 Austrian Institute of Technology -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TEST_TOLERANCES_UTILS_HPP_ -#define TEST_TOLERANCES_UTILS_HPP_ - -#include - -#include "gmock/gmock.h" - -#include "joint_trajectory_controller/tolerances.hpp" - -double default_goal_time = 0.1; -double stopped_velocity_tolerance = 0.1; - -void expectDefaultTolerances(joint_trajectory_controller::SegmentTolerances active_tolerances) -{ - // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance - - ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); - - ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); - EXPECT_DOUBLE_EQ( - active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); - EXPECT_DOUBLE_EQ( - active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); - EXPECT_DOUBLE_EQ( - active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); - EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); -} - -#endif // TEST_TOLERANCES_UTILS_HPP_ diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index c626326cc8..fb749ac250 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -47,7 +47,6 @@ #include "trajectory_msgs/msg/joint_trajectory_point.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "test_tolerances_utils.hpp" #include "test_trajectory_controller_utils.hpp" using std::placeholders::_1; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index f0ccacee5b..693e19e67a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -25,6 +25,7 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "joint_trajectory_controller/tolerances.hpp" namespace { @@ -38,6 +39,40 @@ const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; +const double default_goal_time = 0.1; +const double stopped_velocity_tolerance = 0.1; + +[[maybe_unused]] void expectDefaultTolerances( + joint_trajectory_controller::SegmentTolerances active_tolerances) +{ + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance + + ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).velocity, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.state_tolerance.at(2).acceleration, 0.0); + + ASSERT_EQ(active_tolerances.goal_state_tolerance.size(), 3); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(0).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(0).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(1).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(1).acceleration, 0.0); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).position, 0.1); + EXPECT_DOUBLE_EQ( + active_tolerances.goal_state_tolerance.at(2).velocity, stopped_velocity_tolerance); + EXPECT_DOUBLE_EQ(active_tolerances.goal_state_tolerance.at(2).acceleration, 0.0); +} + bool is_same_sign_or_zero(double val1, double val2) { return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); From 34ca3002677c42f72e177c01edd17904836f0c69 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 24 Jun 2024 21:33:03 +0000 Subject: [PATCH 17/17] Update docs --- doc/migration/Jazzy.rst | 1 + doc/release_notes/Jazzy.rst | 14 ++++++++++++++ joint_trajectory_controller/doc/userdoc.rst | 16 +++++++++++++++- 3 files changed, 30 insertions(+), 1 deletion(-) diff --git a/doc/migration/Jazzy.rst b/doc/migration/Jazzy.rst index 72e6a52e6d..2114436098 100644 --- a/doc/migration/Jazzy.rst +++ b/doc/migration/Jazzy.rst @@ -18,3 +18,4 @@ joint_trajectory_controller * Empty trajectory messages are discarded (`#902 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint. +* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 `_). Adaptions to the action goal might be necessary. diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 4c5db18f3e..4793fd957d 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -33,6 +33,20 @@ joint_trajectory_controller * Action field ``error_string`` is now filled with meaningful strings (`#887 `_). * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 `_). * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 `_). ``angle_wraparound`` parameter was completely removed. +* Tolerances sent with the action goal are now processed and used for the action. (`#716 `_). For details, see the `JointTolerance message `_: + + .. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. pid_controller ************************ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index af495ad14d..4dcb71a064 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -152,7 +152,21 @@ Actions [#f1]_ The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. -Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. +Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message `_: + +.. code-block:: markdown + + The tolerances specify the amount the position, velocity, and + accelerations can vary from the setpoints. For example, in the case + of trajectory control, when the actual position varies beyond + (desired position + position tolerance), the trajectory goal may + abort. + + There are two special values for tolerances: + * 0 - The tolerance is unspecified and will remain at whatever the default is + * -1 - The tolerance is "erased". If there was a default, the joint will be + allowed to move without restriction. + When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held.