Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[JTC] Make goal_time_tolerance overwrite default value only if explicitly set (backport #1192 + #1209) #1208

Merged
merged 2 commits into from
Jul 16, 2024
Merged

[JTC] Make goal_time_tolerance overwrite default value only if explicitly set (backport #1192 + #1209) #1208

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
57ada6f
[JTC] Make goal_time_tolerance overwrite default value only if explic…
fmauch Jul 9, 2024
9fbd7fe
Fix test_tolerances_via_actions
christophfroehlich Jul 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
186 changes: 72 additions & 114 deletions joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp
View file
Open in desktop
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
Open in desktop
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
Loading