Skip to content

Commit 403b95c

Browse files
DavidG-DevelopDavidG-Develop
authored
Adaptive tolerance goal checker (#910)
* add docs for adaptive tolerance goal checker Signed-off-by: David G <david.randommail1@gmail.com> * fix pre commit Signed-off-by: David G <david.randommail1@gmail.com> * add axis goal checker entry Signed-off-by: David G <david.randommail1@gmail.com> * edit creators on axis goal checker Signed-off-by: David G <david.randommail1@gmail.com> --------- Signed-off-by: David G <david.randommail1@gmail.com>
1 parent e1fff12 commit 403b95c

6 files changed

Lines changed: 684 additions & 0 deletions

File tree

‎configuration/packages/configuring-controller-server.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -305,6 +305,7 @@ Provided Plugins
305305
nav2_controller-plugins/simple_progress_checker.rst
306306
nav2_controller-plugins/pose_progress_checker.rst
307307
nav2_controller-plugins/axis_goal_checker.rst
308+
nav2_controller-plugins/adaptive_tolerance_goal_checker.rst
308309
nav2_controller-plugins/simple_goal_checker.rst
309310
nav2_controller-plugins/stopped_goal_checker.rst
310311
nav2_controller-plugins/position_goal_checker.rst

‎configuration/packages/nav2_controller-plugins/adaptive_tolerance_goal_checker.rst‎

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
.. _configuring_nav2_controller_adaptive_tolerance_goal_checker_plugin:
2+
3+
AdaptiveToleranceGoalChecker
4+
============================
5+
6+
The adaptive tolerance goal checker uses two underlying goal tolerances a fine and a coarse one. The fine tolerance is used to instantly trigger goal reached when the robot is close to the goal (functionally same as simple goal checker), while the coarse tolerance is used to trigger goal reached when the robot is further from the goal but is making no meaningful progress towards it (or not expected to).
7+
8+
The goal is cosidered reached when one of the following conditions is met:
9+
- The robot is within the fine goal tolerance
10+
- The robot is within the coarse goal tolerance and its linear velocity and orientational velocity are below a specified threshold for a set amount of cycles
11+
- The robot is within the coarse goal tolerance and robots distance to the goal is not improving for a set amount of cycles
12+
- The robot is within the coarse goal tolerance and it has passed the finish line (the line perpendicular to the first robot pose within the coarse tolerance and passing through the goal pose)
13+
14+
.. image:: /images/adaptive_tolerance_goal_checker.png
15+
:alt: AdaptiveToleranceGoalChecker Illustration
16+
:align: center
17+
18+
Parameters
19+
**********
20+
21+
``<nav2_controller plugin>``: nav2_controller plugin name defined in the **goal_checker_plugin_id** parameter in :ref:`configuring_controller_server`.
22+
23+
:``<nav2_controller plugin>``.fine_xy_goal_tolerance:
24+
25+
====== =======
26+
Type Default
27+
------ -------
28+
double 0.10
29+
====== =======
30+
31+
Description
32+
Fine (desired) XY tolerance to the goal (m). When the robot's XY distance to the goal is within this tolerance, the goal is considered reached immediately (subject to the yaw check). Should be smaller than ``coarse_xy_goal_tolerance``.
33+
34+
:``<nav2_controller plugin>``.coarse_xy_goal_tolerance:
35+
36+
====== =======
37+
Type Default
38+
------ -------
39+
double 0.25
40+
====== =======
41+
42+
Description
43+
Coarse (fallback) XY tolerance to the goal (m). When the robot is within this tolerance but outside the fine tolerance, the goal is considered reached only if one of the coarse-tier acceptance conditions fires (stopped stagnation, distance stagnation, or finish-line crossing). Should be larger than ``fine_xy_goal_tolerance``.
44+
45+
:``<nav2_controller plugin>``.yaw_goal_tolerance:
46+
47+
====== =======
48+
Type Default
49+
------ -------
50+
double 0.25
51+
====== =======
52+
53+
Description
54+
Tolerance on the yaw angle to the goal orientation (rad). The goal is only considered reached once the XY check passes AND the yaw error is within this tolerance.
55+
56+
:``<nav2_controller plugin>``.path_length_tolerance:
57+
58+
====== =======
59+
Type Default
60+
------ -------
61+
double 1.0
62+
====== =======
63+
64+
Description
65+
Maximum remaining transformed global plan length above which the goal check is skipped (m). Prevents premature goal acceptance when the robot is still far from the goal along the path.
66+
67+
:``<nav2_controller plugin>``.stateful:
68+
69+
==== =======
70+
Type Default
71+
---- -------
72+
bool true
73+
==== =======
74+
75+
Description
76+
If true, once the XY tolerance is satisfied the checker latches that result and only re-evaluates yaw on subsequent cycles. If false, both XY and yaw are re-checked every cycle.
77+
78+
:``<nav2_controller plugin>``.symmetric_yaw_tolerance:
79+
80+
==== =======
81+
Type Default
82+
---- -------
83+
bool false
84+
==== =======
85+
86+
Description
87+
If true, the yaw check accepts both the goal orientation and its 180° opposite (useful for symmetric robots that can drive forward or backward). If false, only the goal orientation is accepted.
88+
89+
:``<nav2_controller plugin>``.trans_stopped_velocity:
90+
91+
====== =======
92+
Type Default
93+
------ -------
94+
double 0.10
95+
====== =======
96+
97+
Description
98+
Linear velocity threshold below which the robot is considered "stopped" for the stopped-stagnation acceptance path (m/s). Combined with ``rot_stopped_velocity`` and ``required_stagnation_cycles``.
99+
100+
:``<nav2_controller plugin>``.rot_stopped_velocity:
101+
102+
====== =======
103+
Type Default
104+
------ -------
105+
double 0.10
106+
====== =======
107+
108+
Description
109+
Angular velocity threshold below which the robot is considered "stopped" for the stopped-stagnation acceptance path (rad/s). Combined with ``trans_stopped_velocity`` and ``required_stagnation_cycles``.
110+
111+
:``<nav2_controller plugin>``.required_stagnation_cycles:
112+
113+
==== =======
114+
Type Default
115+
---- -------
116+
int 15
117+
==== =======
118+
119+
Description
120+
Number of consecutive controller cycles for which a stagnation condition must hold before the coarse-tier acceptance fires. Applies to both stopped stagnation (velocity below threshold) and distance stagnation (no improvement in distance to goal). Must be ``>= 1``.
121+
122+
Example
123+
*******
124+
125+
.. code-block:: yaml
126+
127+
controller_server:
128+
ros__parameters:
129+
goal_checker_plugins: ["goal_checker"]
130+
goal_checker:
131+
plugin: "nav2_controller::AdaptiveToleranceGoalChecker"
132+
fine_xy_goal_tolerance: 0.10
133+
coarse_xy_goal_tolerance: 0.25
134+
yaw_goal_tolerance: 0.25
135+
path_length_tolerance: 1.0
136+
stateful: true
137+
symmetric_yaw_tolerance: false
138+
trans_stopped_velocity: 0.10
139+
rot_stopped_velocity: 0.10
140+
required_stagnation_cycles: 15

0 commit comments

Comments
 (0)