Skip to content

Commit c5506f7

Browse files
christophfroehlichmergify[bot]
christophfroehlich
authored and
mergify[bot]
committed
[JTC] Process tolerances sent with action goal (#716)
(cherry picked from commit 07061f9) # Conflicts: # doc/migration/Jazzy.rst # doc/release_notes/Jazzy.rst
1 parent 485c561 commit c5506f7

File tree

10 files changed

+989
-34
lines changed

10 files changed

+989
-34
lines changed

‎doc/migration/Jazzy.rst

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/migration/Jazzy.rst
2+
3+
Iron to Jazzy
4+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
5+
This list summarizes important changes between Iron (previous) and Jazzy (current) releases, where changes to user code might be necessary.
6+
7+
8+
diff_drive_controller
9+
*****************************
10+
* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 <https://github.com/ros-controls/ros2_controllers/pull/812>`_).
11+
12+
joint_trajectory_controller
13+
*****************************
14+
15+
* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 <https://github.com/ros-controls/ros2_controllers/pull/834>`_).
16+
* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 <https://github.com/ros-controls/ros2_controllers/pull/839>`_).
17+
* Goals are now cancelled in ``on_deactivate`` transition (`#962 <https://github.com/ros-controls/ros2_controllers/pull/962>`_).
18+
* Empty trajectory messages are discarded (`#902 <https://github.com/ros-controls/ros2_controllers/pull/902>`_).
19+
* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
20+
* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint.
21+
* Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). Adaptions to the action goal might be necessary.

‎doc/release_notes/Jazzy.rst

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Jazzy.rst
2+
3+
Iron to Jazzy
4+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
5+
This list summarizes the changes between Iron (previous) and Jazzy (current) releases.
6+
7+
admittance_controller
8+
************************
9+
* Remove ``robot_description`` parameter from parameter YAML, because it is not used at all (`#963 <https://github.com/ros-controls/ros2_controllers/pull/963>`_).
10+
11+
diff_drive_controller
12+
*****************************
13+
* The twist message on ``~/cmd_vel`` is now required to be of stamped type (`#812 <https://github.com/ros-controls/ros2_controllers/pull/812>`_).
14+
* Remove unused parameter ``wheels_per_side`` (`#958 <https://github.com/ros-controls/ros2_controllers/pull/958>`_).
15+
16+
joint_trajectory_controller
17+
*****************************
18+
19+
* Parameter ``allow_nonzero_velocity_at_trajectory_end`` is now per default ``false`` (`#834 <https://github.com/ros-controls/ros2_controllers/pull/834>`_).
20+
* Activate update of dynamic parameters (`#761 <https://github.com/ros-controls/ros2_controllers/pull/761>`_ and `#849 <https://github.com/ros-controls/ros2_controllers/pull/849>`_).
21+
* The parameter ``start_with_holding`` is removed, it now always holds the position at activation (`#839 <https://github.com/ros-controls/ros2_controllers/pull/839>`_).
22+
* Continue with last trajectory-point on success, instead of hold-position from current state (`#842 <https://github.com/ros-controls/ros2_controllers/pull/842>`_).
23+
* Add console output for tolerance checks (`#932 <https://github.com/ros-controls/ros2_controllers/pull/932>`_):
24+
25+
.. code::
26+
27+
[tolerances]: State tolerances failed for joint 2:
28+
[tolerances]: Position Error: 0.020046, Position Tolerance: 0.010000
29+
[trajectory_controllers]: Aborted due goal_time_tolerance exceeding by 1.010000 seconds
30+
31+
* Goals are now cancelled in ``on_deactivate`` transition (`#962 <https://github.com/ros-controls/ros2_controllers/pull/962>`_).
32+
* Empty trajectory messages are discarded (`#902 <https://github.com/ros-controls/ros2_controllers/pull/902>`_).
33+
* Action field ``error_string`` is now filled with meaningful strings (`#887 <https://github.com/ros-controls/ros2_controllers/pull/887>`_).
34+
* Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
35+
* The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). ``angle_wraparound`` parameter was completely removed.
36+
* Tolerances sent with the action goal are now processed and used for the action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
37+
38+
.. code-block:: markdown
39+
40+
The tolerances specify the amount the position, velocity, and
41+
accelerations can vary from the setpoints. For example, in the case
42+
of trajectory control, when the actual position varies beyond
43+
(desired position + position tolerance), the trajectory goal may
44+
abort.
45+
46+
There are two special values for tolerances:
47+
* 0 - The tolerance is unspecified and will remain at whatever the default is
48+
* -1 - The tolerance is "erased". If there was a default, the joint will be
49+
allowed to move without restriction.
50+
51+
pid_controller
52+
************************
53+
* 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).
54+
55+
steering_controllers_library
56+
********************************
57+
* Changing default int values to double in steering controller's yaml file. The controllers should now initialize successfully without specifying these parameters (`#927 <https://github.com/ros-controls/ros2_controllers/pull/927>`_).
58+
* A fix for Ackermann steering odometry was added (`#921 <https://github.com/ros-controls/ros2_controllers/pull/921>`_).
59+
60+
tricycle_controller
61+
************************
62+
* tricycle_controller now uses generate_parameter_library (`#957 <https://github.com/ros-controls/ros2_controllers/pull/957>`_).

‎joint_trajectory_controller/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,10 @@ if(BUILD_TESTING)
6060
target_link_libraries(test_trajectory joint_trajectory_controller)
6161
target_compile_definitions(test_trajectory PRIVATE _USE_MATH_DEFINES)
6262

63+
ament_add_gmock(test_tolerances test/test_tolerances.cpp)
64+
target_link_libraries(test_tolerances joint_trajectory_controller)
65+
target_compile_definitions(test_tolerances PRIVATE _USE_MATH_DEFINES)
66+
6367
ament_add_gmock(test_trajectory_controller
6468
test/test_trajectory_controller.cpp)
6569
set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)

‎joint_trajectory_controller/doc/userdoc.rst

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -152,7 +152,21 @@ Actions [#f1]_
152152

153153
The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired.
154154

155-
Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances.
155+
Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. For details, see the `JointTolerance message <https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg>`_:
156+
157+
.. code-block:: markdown
158+
159+
The tolerances specify the amount the position, velocity, and
160+
accelerations can vary from the setpoints. For example, in the case
161+
of trajectory control, when the actual position varies beyond
162+
(desired position + position tolerance), the trajectory goal may
163+
abort.
164+
165+
There are two special values for tolerances:
166+
* 0 - The tolerance is unspecified and will remain at whatever the default is
167+
* -1 - The tolerance is "erased". If there was a default, the joint will be
168+
allowed to move without restriction.
169+
156170
When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`).
157171
If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held.
158172

‎joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -243,7 +243,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
243243
size_t joint_names_size, const std::vector<double> & vector_field,
244244
const std::string & string_for_vector_field, size_t i, bool allow_empty) const;
245245

246+
// the tolerances from the node parameter
246247
SegmentTolerances default_tolerances_;
248+
// the tolerances used for the current goal
249+
realtime_tools::RealtimeBuffer<SegmentTolerances> active_tolerances_;
247250

248251
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
249252
void preempt_active_goal();

‎joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

Lines changed: 215 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -30,12 +30,15 @@
3030
#ifndef JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
3131
#define JOINT_TRAJECTORY_CONTROLLER__TOLERANCES_HPP_
3232

33+
#include <limits>
34+
#include <string>
3335
#include <vector>
3436

3537
#include "control_msgs/action/follow_joint_trajectory.hpp"
3638
#include "joint_trajectory_controller_parameters.hpp"
3739

3840
#include "rclcpp/node.hpp"
41+
#include "rclcpp/time.hpp"
3942

4043
namespace joint_trajectory_controller
4144
{
@@ -85,16 +88,19 @@ struct SegmentTolerances
8588
* goal: 0.01
8689
* \endcode
8790
*
91+
* \param jtc_logger The logger to use for output
8892
* \param params The ROS Parameters
8993
* \return Trajectory segment tolerances.
9094
*/
91-
SegmentTolerances get_segment_tolerances(Params const & params)
95+
SegmentTolerances get_segment_tolerances(rclcpp::Logger & jtc_logger, const Params & params)
9296
{
9397
auto const & constraints = params.constraints;
9498
auto const n_joints = params.joints.size();
9599

96100
SegmentTolerances tolerances;
97101
tolerances.goal_time_tolerance = constraints.goal_time;
102+
static auto logger = jtc_logger.get_child("tolerance");
103+
RCLCPP_DEBUG(logger, "goal_time %f", constraints.goal_time);
98104

99105
// State and goal state tolerances
100106
tolerances.state_tolerance.resize(n_joints);
@@ -106,16 +112,221 @@ SegmentTolerances get_segment_tolerances(Params const & params)
106112
tolerances.goal_state_tolerance[i].position = constraints.joints_map.at(joint).goal;
107113
tolerances.goal_state_tolerance[i].velocity = constraints.stopped_velocity_tolerance;
108114

109-
auto logger = rclcpp::get_logger("tolerance");
110115
RCLCPP_DEBUG(
111-
logger, "%s %f", (joint + ".trajectory").c_str(), tolerances.state_tolerance[i].position);
116+
logger, "%s %f", (joint + ".trajectory.position").c_str(),
117+
tolerances.state_tolerance[i].position);
112118
RCLCPP_DEBUG(
113-
logger, "%s %f", (joint + ".goal").c_str(), tolerances.goal_state_tolerance[i].position);
119+
logger, "%s %f", (joint + ".goal.position").c_str(),
120+
tolerances.goal_state_tolerance[i].position);
121+
RCLCPP_DEBUG(
122+
logger, "%s %f", (joint + ".goal.velocity").c_str(),
123+
tolerances.goal_state_tolerance[i].velocity);
114124
}
115125

116126
return tolerances;
117127
}
118128

129+
/**
130+
* \brief Populate trajectory segment tolerances using data from an action goal.
131+
*
132+
* \param jtc_logger The logger to use for output
133+
* \param default_tolerances The default tolerances to use if the action goal does not specify any.
134+
* \param goal The new action goal
135+
* \param joints The joints configured by ROS parameters
136+
* \return Trajectory segment tolerances.
137+
*/
138+
SegmentTolerances get_segment_tolerances(
139+
rclcpp::Logger & jtc_logger, const SegmentTolerances & default_tolerances,
140+
const control_msgs::action::FollowJointTrajectory::Goal & goal,
141+
const std::vector<std::string> & joints)
142+
{
143+
SegmentTolerances active_tolerances(default_tolerances);
144+
145+
active_tolerances.goal_time_tolerance = rclcpp::Duration(goal.goal_time_tolerance).seconds();
146+
static auto logger = jtc_logger.get_child("tolerance");
147+
RCLCPP_DEBUG(logger, "%s %f", "goal_time", active_tolerances.goal_time_tolerance);
148+
149+
// from
150+
// https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointTolerance.msg
151+
// There are two special values for tolerances:
152+
// * 0 - The tolerance is unspecified and will remain at whatever the default is
153+
// * -1 - The tolerance is "erased".
154+
// If there was a default, the joint will be allowed to move without restriction.
155+
constexpr double ERASE_VALUE = -1.0;
156+
auto is_erase_value = [](double value)
157+
{ return fabs(value - ERASE_VALUE) < std::numeric_limits<float>::epsilon(); };
158+
159+
// State and goal state tolerances
160+
for (auto joint_tol : goal.path_tolerance)
161+
{
162+
auto const joint = joint_tol.name;
163+
// map joint names from goal to active_tolerances
164+
auto it = std::find(joints.begin(), joints.end(), joint);
165+
if (it == joints.end())
166+
{
167+
RCLCPP_ERROR(
168+
logger, "%s",
169+
("joint '" + joint +
170+
"' specified in goal.path_tolerance does not exist. "
171+
"Using default tolerances.")
172+
.c_str());
173+
return default_tolerances;
174+
}
175+
auto i = std::distance(joints.cbegin(), it);
176+
if (joint_tol.position > 0.0)
177+
{
178+
active_tolerances.state_tolerance[i].position = joint_tol.position;
179+
}
180+
else if (is_erase_value(joint_tol.position))
181+
{
182+
active_tolerances.state_tolerance[i].position = 0.0;
183+
}
184+
else if (joint_tol.position < 0.0)
185+
{
186+
RCLCPP_ERROR(
187+
logger, "%s",
188+
("joint '" + joint +
189+
"' specified in goal.path_tolerance has a invalid position tolerance. "
190+
"Using default tolerances.")
191+
.c_str());
192+
return default_tolerances;
193+
}
194+
195+
if (joint_tol.velocity > 0.0)
196+
{
197+
active_tolerances.state_tolerance[i].velocity = joint_tol.velocity;
198+
}
199+
else if (is_erase_value(joint_tol.velocity))
200+
{
201+
active_tolerances.state_tolerance[i].velocity = 0.0;
202+
}
203+
else if (joint_tol.velocity < 0.0)
204+
{
205+
RCLCPP_ERROR(
206+
logger, "%s",
207+
("joint '" + joint +
208+
"' specified in goal.path_tolerance has a invalid velocity tolerance. "
209+
"Using default tolerances.")
210+
.c_str());
211+
return default_tolerances;
212+
}
213+
214+
if (joint_tol.acceleration > 0.0)
215+
{
216+
active_tolerances.state_tolerance[i].acceleration = joint_tol.acceleration;
217+
}
218+
else if (is_erase_value(joint_tol.acceleration))
219+
{
220+
active_tolerances.state_tolerance[i].acceleration = 0.0;
221+
}
222+
else if (joint_tol.acceleration < 0.0)
223+
{
224+
RCLCPP_ERROR(
225+
logger, "%s",
226+
("joint '" + joint +
227+
"' specified in goal.path_tolerance has a invalid acceleration tolerance. "
228+
"Using default tolerances.")
229+
.c_str());
230+
return default_tolerances;
231+
}
232+
233+
RCLCPP_DEBUG(
234+
logger, "%s %f", (joint + ".state_tolerance.position").c_str(),
235+
active_tolerances.state_tolerance[i].position);
236+
RCLCPP_DEBUG(
237+
logger, "%s %f", (joint + ".state_tolerance.velocity").c_str(),
238+
active_tolerances.state_tolerance[i].velocity);
239+
RCLCPP_DEBUG(
240+
logger, "%s %f", (joint + ".state_tolerance.acceleration").c_str(),
241+
active_tolerances.state_tolerance[i].acceleration);
242+
}
243+
for (auto goal_tol : goal.goal_tolerance)
244+
{
245+
auto const joint = goal_tol.name;
246+
// map joint names from goal to active_tolerances
247+
auto it = std::find(joints.begin(), joints.end(), joint);
248+
if (it == joints.end())
249+
{
250+
RCLCPP_ERROR(
251+
logger, "%s",
252+
("joint '" + joint +
253+
"' specified in goal.goal_tolerance does not exist. "
254+
"Using default tolerances.")
255+
.c_str());
256+
return default_tolerances;
257+
}
258+
auto i = std::distance(joints.cbegin(), it);
259+
if (goal_tol.position > 0.0)
260+
{
261+
active_tolerances.goal_state_tolerance[i].position = goal_tol.position;
262+
}
263+
else if (is_erase_value(goal_tol.position))
264+
{
265+
active_tolerances.goal_state_tolerance[i].position = 0.0;
266+
}
267+
else if (goal_tol.position < 0.0)
268+
{
269+
RCLCPP_ERROR(
270+
logger, "%s",
271+
("joint '" + joint +
272+
"' specified in goal.goal_tolerance has a invalid position tolerance. "
273+
"Using default tolerances.")
274+
.c_str());
275+
return default_tolerances;
276+
}
277+
278+
if (goal_tol.velocity > 0.0)
279+
{
280+
active_tolerances.goal_state_tolerance[i].velocity = goal_tol.velocity;
281+
}
282+
else if (is_erase_value(goal_tol.velocity))
283+
{
284+
active_tolerances.goal_state_tolerance[i].velocity = 0.0;
285+
}
286+
else if (goal_tol.velocity < 0.0)
287+
{
288+
RCLCPP_ERROR(
289+
logger, "%s",
290+
("joint '" + joint +
291+
"' specified in goal.goal_tolerance has a invalid velocity tolerance. "
292+
"Using default tolerances.")
293+
.c_str());
294+
return default_tolerances;
295+
}
296+
297+
if (goal_tol.acceleration > 0.0)
298+
{
299+
active_tolerances.goal_state_tolerance[i].acceleration = goal_tol.acceleration;
300+
}
301+
else if (is_erase_value(goal_tol.acceleration))
302+
{
303+
active_tolerances.goal_state_tolerance[i].acceleration = 0.0;
304+
}
305+
else if (goal_tol.acceleration < 0.0)
306+
{
307+
RCLCPP_ERROR(
308+
logger, "%s",
309+
("joint '" + joint +
310+
"' specified in goal.goal_tolerance has a invalid acceleration tolerance. "
311+
"Using default tolerances.")
312+
.c_str());
313+
return default_tolerances;
314+
}
315+
316+
RCLCPP_DEBUG(
317+
logger, "%s %f", (joint + ".goal_state_tolerance.position").c_str(),
318+
active_tolerances.goal_state_tolerance[i].position);
319+
RCLCPP_DEBUG(
320+
logger, "%s %f", (joint + ".goal_state_tolerance.velocity").c_str(),
321+
active_tolerances.goal_state_tolerance[i].velocity);
322+
RCLCPP_DEBUG(
323+
logger, "%s %f", (joint + ".goal_state_tolerance.acceleration").c_str(),
324+
active_tolerances.goal_state_tolerance[i].acceleration);
325+
}
326+
327+
return active_tolerances;
328+
}
329+
119330
/**
120331
* \param state_error State error to check.
121332
* \param joint_idx Joint index for the state error

0 commit comments

Comments
 (0)