Skip to content

Commit

Permalink
[JTC] Make goal_time_tolerance overwrite default value only if explic…
Browse files Browse the repository at this point in the history 
…itly set (backport #1192 + #1209) (#1207)

---------

Co-authored-by: Felix Exner (fexner) <exner@fzi.de>
Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>
  • Loading branch information
@mergify @fmauch @christophfroehlich
3 people authored Jul 16, 2024
1 parent f533cfd commit c728db8
Show file tree
Hide file tree
Showing 4 changed files with 107 additions and 131 deletions.
186 changes: 72 additions & 114 deletions joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_

#include <limits>
#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -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<float>::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.
*
Expand All @@ -141,20 +169,22 @@ SegmentTolerances get_segment_tolerances(
const std::vector<std::string> & 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<float>::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)
Expand All @@ -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;
}

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

Expand Down
39 changes: 28 additions & 11 deletions joint_trajectory_controller/test/test_tolerances.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,14 @@ using trajectory_msgs::msg::JointTrajectoryPoint;
std::vector<std::string> joint_names_ = {"joint1", "joint2", "joint3"};

control_msgs::action::FollowJointTrajectory_Goal prepareGoalMsg(
const std::vector<JointTrajectoryPoint> & points, double timeout,
const std::vector<JointTrajectoryPoint> & points, double goal_time_tolerance,
const std::vector<control_msgs::msg::JointTolerance> path_tolerance =
std::vector<control_msgs::msg::JointTolerance>(),
const std::vector<control_msgs::msg::JointTolerance> goal_tolerance =
std::vector<control_msgs::msg::JointTolerance>())
{
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_;
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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<JointTrajectoryPoint> 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<control_msgs::msg::JointTolerance> 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<JointTrajectoryPoint> points;
Expand All @@ -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);
}
{
Expand All @@ -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);
}
{
Expand All @@ -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);
}
{
Expand All @@ -317,7 +339,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), 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);
}
{
Expand All @@ -344,7 +365,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), 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);
}
{
Expand All @@ -371,7 +391,6 @@ TEST_F(TestTolerancesFixture, test_invalid_tolerances)
prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), 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);
}
}
Expand All @@ -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)
Expand All @@ -419,6 +437,5 @@ TEST_F(TestTolerancesFixture, test_invalid_joints_goal_tolerance)
prepareGoalMsg(points, 3.0, std::vector<control_msgs::msg::JointTolerance>(), 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);
}
Loading

0 comments on commit c728db8

Please sign in to comment.