From 9121ae20a0ae35013a1187e7a13f0e7adaf239a6 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 9 Jul 2024 09:08:28 +0200 Subject: [PATCH 1/2] [JTC] Make goal_time_tolerance overwrite default value only if explicitly set (#1192) (cherry picked from commit 776c432dbc9a8db11119823803daad9525c63846) --- .../tolerances.hpp | 186 +++++++----------- .../test/test_tolerances.cpp | 35 +++- .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 99 insertions(+), 123 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 1998930182..c4404920df 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 @@ -126,6 +127,33 @@ SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Para return tolerances; } +double resolve_tolerance_source(const double default_value, const double goal_value) +{ + // 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. + constexpr double ERASE_VALUE = -1.0; + auto is_erase_value = [](double value) + { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + + if (goal_value > 0.0) + { + return goal_value; + } + else if (is_erase_value(goal_value)) + { + return 0.0; + } + else if (goal_value < 0.0) + { + throw std::runtime_error("Illegal tolerance value."); + } + return default_value; +} + /** * \brief Populate trajectory segment tolerances using data from an action goal. * @@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances( const std::vector & joints) { SegmentTolerances active_tolerances(default_tolerances); - - active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds(); static auto logger = jtc_logger.get_child("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. - constexpr double ERASE_VALUE = -1.0; - auto is_erase_value = [](double value) - { return fabs(value - ERASE_VALUE) < std::numeric_limits::epsilon(); }; + try + { + active_tolerances.goal_time_tolerance = resolve_tolerance_source( + default_tolerances.goal_time_tolerance, rclcpp::Duration(goal.goal_time_tolerance).seconds()); + } + catch (const std::runtime_error & e) + { + RCLCPP_ERROR_STREAM( + logger, "Specified illegal goal_time_tolerance: " + << rclcpp::Duration(goal.goal_time_tolerance).seconds() + << ". Using default tolerances"); + return default_tolerances; + } + RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance); // State and goal state tolerances for (auto joint_tol : goal.path_tolerance) @@ -173,60 +203,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (joint_tol.position > 0.0) - { - active_tolerances.state_tolerance[i].position = joint_tol.position; - } - else if (is_erase_value(joint_tol.position)) + std::string interface = ""; + try { - active_tolerances.state_tolerance[i].position = 0.0; + interface = "position"; + active_tolerances.state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.state_tolerance[i].position, joint_tol.position); + interface = "velocity"; + active_tolerances.state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.state_tolerance[i].velocity, joint_tol.velocity); + interface = "acceleration"; + active_tolerances.state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.state_tolerance[i].acceleration, joint_tol.acceleration); } - else if (joint_tol.position < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.path_tolerance has a invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.velocity > 0.0) - { - active_tolerances.state_tolerance[i].velocity = joint_tol.velocity; - } - else if (is_erase_value(joint_tol.velocity)) - { - 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 invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (joint_tol.acceleration > 0.0) - { - active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration; - } - else if (is_erase_value(joint_tol.acceleration)) - { - 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 invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.path_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } @@ -256,60 +250,24 @@ SegmentTolerances get_segment_tolerances( return default_tolerances; } auto i = std::distance(joints.cbegin(), it); - if (goal_tol.position > 0.0) - { - active_tolerances.goal_state_tolerance[i].position = goal_tol.position; - } - else if (is_erase_value(goal_tol.position)) - { - 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 invalid position tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.velocity > 0.0) - { - active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity; - } - else if (is_erase_value(goal_tol.velocity)) + std::string interface = ""; + try { - active_tolerances.goal_state_tolerance[i].velocity = 0.0; + interface = "position"; + active_tolerances.goal_state_tolerance[i].position = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].position, goal_tol.position); + interface = "velocity"; + active_tolerances.goal_state_tolerance[i].velocity = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].velocity, goal_tol.velocity); + interface = "acceleration"; + active_tolerances.goal_state_tolerance[i].acceleration = resolve_tolerance_source( + default_tolerances.goal_state_tolerance[i].acceleration, goal_tol.acceleration); } - else if (goal_tol.velocity < 0.0) + catch (const std::runtime_error & e) { - RCLCPP_ERROR( - logger, "%s", - ("joint '" + joint + - "' specified in goal.goal_tolerance has a invalid velocity tolerance. " - "Using default tolerances.") - .c_str()); - return default_tolerances; - } - - if (goal_tol.acceleration > 0.0) - { - active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration; - } - else if (is_erase_value(goal_tol.acceleration)) - { - 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 invalid acceleration tolerance. " - "Using default tolerances.") - .c_str()); + RCLCPP_ERROR_STREAM( + logger, "joint '" << joint << "' specified in goal.goal_tolerance has a invalid " + << interface << " tolerance. Using default tolerances."); return default_tolerances; } diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index 66914b6da4..af5036b869 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -183,7 +183,7 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) path_tolerance.push_back(tolerance); goal_tolerance.push_back(tolerance); - auto goal_msg = prepareGoalMsg(points, 0.0, path_tolerance, goal_tolerance); + auto goal_msg = prepareGoalMsg(points, -1.0, path_tolerance, goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); @@ -215,6 +215,31 @@ TEST_F(TestTolerancesFixture, test_deactivate_tolerances) // send goal with invalid tolerances, are the default ones used? TEST_F(TestTolerancesFixture, test_invalid_tolerances) { + { + SCOPED_TRACE("negative goal_time_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 = 0.0; + path_tolerance.push_back(tolerance); + + auto goal_msg = prepareGoalMsg(points, -123.0, path_tolerance); + auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( + logger, default_tolerances, goal_msg, params.joints); + expectDefaultTolerances(active_tolerances); + } { SCOPED_TRACE("negative path position tolerance"); std::vector points; @@ -238,7 +263,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -264,7 +288,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -290,7 +313,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) auto goal_msg = prepareGoalMsg(points, 3.0, path_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } { @@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } } @@ -395,7 +414,6 @@ 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( logger, default_tolerances, goal_msg, params.joints); - EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); expectDefaultTolerances(active_tolerances); } TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) @@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance) prepareGoalMsg(points, 3.0, std::vector(), goal_tolerance); auto active_tolerances = joint_trajectory_controller::get_segment_tolerances( logger, default_tolerances, goal_msg, params.joints); - 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 731448a85c..3057c5b3a6 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -45,6 +45,7 @@ const double stopped_velocity_tolerance = 0.1; [[maybe_unused]] void expectDefaultTolerances( joint_trajectory_controller::SegmentTolerances active_tolerances) { + EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3); From 7a62ba333620aedc6c6ca2040cc8cb613a31c2f8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 16 Jul 2024 07:22:53 +0000 Subject: [PATCH 2/2] Fix test_tolerances_via_actions --- joint_trajectory_controller/test/test_tolerances.cpp | 4 ++-- .../test/test_trajectory_actions.cpp | 11 +++++------ .../test/test_trajectory_controller_utils.hpp | 1 + 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_tolerances.cpp b/joint_trajectory_controller/test/test_tolerances.cpp index af5036b869..bd6304df29 100644 --- a/joint_trajectory_controller/test/test_tolerances.cpp +++ b/joint_trajectory_controller/test/test_tolerances.cpp @@ -32,14 +32,14 @@ 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 & points, double goal_time_tolerance, 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_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fb749ac250..3e65016403 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -153,14 +153,15 @@ 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 goal_time_tolerance, + 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_time_tolerance = rclcpp::Duration::from_seconds(goal_time_tolerance); goal_msg.goal_tolerance = goal_tolerance; goal_msg.path_tolerance = path_tolerance; goal_msg.trajectory.joint_names = joint_names_; @@ -529,7 +530,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -539,7 +540,6 @@ 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_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); expectDefaultTolerances(active_tolerances); } @@ -639,7 +639,7 @@ TEST_F(TestTrajectoryActions, test_tolerances_via_actions) point.positions[2] = 3.0; points.push_back(point); - gh_future = sendActionGoal(points, 1.0, goal_options_); + gh_future = sendActionGoal(points, 0.0, goal_options_); } controller_hw_thread_.join(); @@ -649,7 +649,6 @@ 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_DOUBLE_EQ(active_tolerances.goal_time_tolerance, 1.0); 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 3057c5b3a6..49169215c2 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -46,6 +46,7 @@ const double stopped_velocity_tolerance = 0.1; joint_trajectory_controller::SegmentTolerances active_tolerances) { EXPECT_DOUBLE_EQ(active_tolerances.goal_time_tolerance, default_goal_time); + // acceleration is never set, and goal_state_tolerance.velocity from stopped_velocity_tolerance ASSERT_EQ(active_tolerances.state_tolerance.size(), 3);