diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 204cd531f2..a6b016cbf0 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 d200008f7b..243c87725e 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);