Skip to content

Commit 0f218c6

Browse files
SteveMacenskiSteveMacenski
authored
New Route Server Demo BT Node Docs (#677)
* config for new nodes Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * sp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent c1905b4 commit 0f218c6

15 files changed

Lines changed: 521 additions & 2 deletions

‎behavior_trees/index.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ Nav2 Behavior Trees
1212
trees/nav_through_poses_recovery.rst
1313
trees/nav_to_pose_and_pause_near_goal_obstacle.rst
1414
trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.rst
15+
trees/navigate_on_route_graph_w_recovery.rst
1516
trees/follow_point.rst
1617
trees/odometry_calibration.rst
1718

‎behavior_trees/trees/navigate_on_route_graph_w_recovery.rst‎

Lines changed: 105 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,105 @@
1+
.. _behavior_tree_navigate_on_route_graph_w_recovery:
2+
3+
Navigate on Route Graph with Recovery
4+
#####################################
5+
6+
This behavior tree implements a different style of navigation than the other versions in this section.
7+
Rather than using a freespace planner ``ComputePathToPose`` to plan a complete path to the goal, this behavior tree instead uses the Route Server to find a route to the goal through a pre-defined navigation graph.
8+
This can be useful for navigating in large-scale environments where real-time planning in freespace for a long distance is not computationally feasible, where a map of the entire space is not possible to plan within, or where deterministic behavior and limited navigation zones/lanes/routes are demanded.
9+
10+
This tree computes a route through the environment using the ``ComputeRoute`` node which is executed on initialization and when either the goal is updated due to preemption (``GlobalUpdatedGoal``) or the current route path is invalid due to collision (``isPathValid``).
11+
After which, if the robot's starting pose is too far from the first route node in the graph solution, it will use freespace planning to connect the robot's current pose to the first node in the route.
12+
This is called the ``first mile`` and is computed using the ``ComputePathToPose`` node.
13+
This may be removed if navigation only on the graph is required and you know that the robot will always be located on or near the graph.
14+
15+
The complimentary action occurs for ``last mile`` where the robot will use freespace planning to connect the last node in the route to the goal pose.
16+
This is done using the ``ComputePathToPose`` node again and similarly can be removed if required.
17+
The compute path, including the first and last mile paths are then smoothed in the smoother server to make the corners more natural and less sharp.
18+
``FollowPath`` is then used to follow this path.
19+
20+
For a detailed description of the role of the selector nodes, recovery behaviors, or fallbacks, see the other behavior tree explanations in this section.
21+
22+
.. code-block:: xml
23+
24+
<root BTCPP_format="4" main_tree_to_execute="MainTree">
25+
<BehaviorTree ID="MainTree">
26+
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
27+
<PipelineSequence name="NavigateWithReplanning">
28+
<ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
29+
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
30+
<RecoveryNode number_of_retries="1" name="ComputeRoute">
31+
<RateController hz="0.5" name="ComputeRouteRateController">
32+
<Fallback>
33+
<!-- Compute a new route if a new goal is found or the path is no longer valid -->
34+
<ReactiveSequence>
35+
<Inverter>
36+
<GlobalUpdatedGoal/>
37+
</Inverter>
38+
<IsPathValid path="{path}"/> <!-- Base it on the complete connected 'path', not simply the 'route_path' -->
39+
</ReactiveSequence>
40+
<Sequence name="ComputeAndSmoothRoute">
41+
<!-- Compute the route -->
42+
<ComputeRoute goal="{goal}" path="{route_path}" route="{route}" use_poses="true" error_code_id="{compute_route_error_code}" error_msg="{compute_route_error_msg}"/>
43+
44+
<!-- Find if the route start node is far from the robot's current pose; if so, connect them for 'first mile'. -->
45+
<ReactiveSequence>
46+
<GetCurrentPose current_pose="{current_pose}"/>
47+
<GetPoseFromPath path="{route_path}" index="0" pose="{route_start_pose}"/>
48+
<Inverter>
49+
<ArePosesNear ref_pose="{current_pose}" target_pose="{route_start_pose}" tolerance="0.3"/>
50+
</Inverter>
51+
<ComputePathToPose goal="{route_start_pose}" path="{first_mile_path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
52+
<ConcatenatePaths input_path1="{first_mile_path}" input_path2="{route_path}" output_path="{route_path}"/>
53+
</ReactiveSequence>
54+
55+
<!-- Find if the route end node is far from the goal pose; if so, connect them for 'last mile'. -->
56+
<ReactiveSequence>
57+
<GetPoseFromPath path="{route_path}" index="-1" pose="{route_end_pose}"/>
58+
<Inverter>
59+
<ArePosesNear ref_pose="{goal}" target_pose="{route_end_pose}" tolerance="0.1"/>
60+
</Inverter>
61+
<ComputePathToPose start="{route_end_pose}" goal="{goal}" path="{last_mile_path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
62+
<ConcatenatePaths input_path1="{route_path}" input_path2="{last_mile_path}" output_path="{route_path}"/>
63+
</ReactiveSequence>
64+
65+
<!-- Smooth the completed route -->
66+
<SmoothPath unsmoothed_path="{route_path}" smoothed_path="{path}" smoother_id="route_smoother" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>
67+
</Sequence>
68+
</Fallback>
69+
</RateController>
70+
<Sequence>
71+
<Fallback>
72+
<WouldARouteRecoveryHelp error_code="{compute_route_error_code}"/>
73+
<WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
74+
</Fallback>
75+
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
76+
</Sequence>
77+
</RecoveryNode>
78+
<RecoveryNode number_of_retries="1" name="FollowPath">
79+
<FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}" error_msg="{follow_path_error_msg}"/>
80+
<Sequence>
81+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
82+
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
83+
</Sequence>
84+
</RecoveryNode>
85+
</PipelineSequence>
86+
<Sequence>
87+
<Fallback>
88+
<WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
89+
<WouldAPlannerRecoveryHelp error_code="{compute_route_error_code}"/>
90+
</Fallback>
91+
<ReactiveFallback name="RecoveryFallback">
92+
<GoalUpdated/>
93+
<RoundRobin name="RecoveryActions">
94+
<Sequence name="ClearingActions">
95+
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
96+
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
97+
</Sequence>
98+
<Wait wait_duration="5.0" error_code_id="{wait_error_code}" error_msg="{wait_error_msg}"/>
99+
<BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_error_code}" error_msg="{backup_error_msg}"/>
100+
</RoundRobin>
101+
</ReactiveFallback>
102+
</Sequence>
103+
</RecoveryNode>
104+
</BehaviorTree>
105+
</root>

‎configuration/packages/bt-plugins/actions/AppendGoalPoseToGoals.rst‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
.. _bt_append_goal_pose_to_goals_action:
2+
3+
AppendGoalPoseToGoals
4+
=====================
5+
6+
Appends a goal ``PoseStamped`` to the end of a ``goals`` vector.
7+
May be useful to add in the final task goal pose to a list of goals extracted from Route nodes (or other sources of future goals).
8+
9+
Input Ports
10+
-----------
11+
12+
:goal_pose:
13+
14+
=============================== =======
15+
Type Default
16+
------------------------------- -------
17+
geometry_msgs/PoseStamped N/A
18+
=============================== =======
19+
20+
Description
21+
Goal pose to append to the ``goals`` vector.
22+
23+
:input_goals:
24+
25+
=============================== =======
26+
Type Default
27+
------------------------------- -------
28+
nav_msgs/Goals N/A
29+
=============================== =======
30+
31+
Description
32+
Input goals vector to append to.
33+
34+
35+
Output Ports
36+
------------
37+
38+
:output_goals:
39+
40+
=============================== =======
41+
Type Default
42+
------------------------------- -------
43+
nav_msgs/Goals N/A
44+
=============================== =======
45+
46+
Description
47+
Output goals vector appended to.
48+
49+
Example
50+
-------
51+
52+
.. code-block:: xml
53+
54+
<AppendGoalPoseToGoals goal_pose="{goal}" input_goals="{goal_poses}" output_goals="{goal_poses}"/>

‎configuration/packages/bt-plugins/actions/ConcatenatePaths.rst‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
.. _bt_concatenate_paths_action:
2+
3+
ConcatenatePaths
4+
================
5+
6+
Concatenates two paths into a single path, in order such that the output is ``input_path1 + input_path2``.
7+
May be used with multiple of these calls sequentially to concatenate multiple paths.
8+
9+
Input Ports
10+
-----------
11+
12+
:input_path1:
13+
14+
=============================== =======
15+
Type Default
16+
------------------------------- -------
17+
nav_msgs/Path N/A
18+
=============================== =======
19+
20+
Description
21+
First path to concatenate.
22+
23+
:input_path2:
24+
25+
=============================== =======
26+
Type Default
27+
------------------------------- -------
28+
nav_msgs/Path N/A
29+
=============================== =======
30+
31+
Description
32+
Second path to concatenate.
33+
34+
Output Ports
35+
------------
36+
37+
:input_path2:
38+
39+
=============================== =======
40+
Type Default
41+
------------------------------- -------
42+
nav_msgs/Path N/A
43+
=============================== =======
44+
45+
Description
46+
Output concatenated path.
47+
48+
Example
49+
-------
50+
51+
.. code-block:: xml
52+
53+
<ConcatenatePaths input_path1="{main_path}" input_path2="{last_mile_path}" output_path="{path}"/>

‎configuration/packages/bt-plugins/actions/ExtractRouteNodesAsGoals.rst‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
.. _bt_extract_route_nodes_as_goals_action:
2+
3+
ExtractRouteNodesAsGoals
4+
========================
5+
6+
Concatenates two paths into a single path, in order such that the output is ``input_path1 + input_path2``.
7+
May be used with multiple of these calls sequentially to concatenate multiple paths.
8+
9+
Input Ports
10+
-----------
11+
12+
:route:
13+
14+
=============================== =======
15+
Type Default
16+
------------------------------- -------
17+
nav2_msgs/Route N/A
18+
=============================== =======
19+
20+
Description
21+
Route to convert its ``nodes`` into Goals.
22+
23+
Output Ports
24+
------------
25+
26+
:goals:
27+
28+
=============================== =======
29+
Type Default
30+
------------------------------- -------
31+
nav_msgs/Goals N/A
32+
=============================== =======
33+
34+
Description
35+
Goals comparing the route's ``nodes``.
36+
37+
Example
38+
-------
39+
40+
.. code-block:: xml
41+
42+
<ExtractRouteNodesAsGoals route="{route}" goals="{route_goals}"/>

‎configuration/packages/bt-plugins/actions/GetCurrentPose.rst‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
.. _bt_get_current_pose_action:
2+
3+
GetCurrentPose
4+
==============
5+
6+
Obtains the current pose from TF and places it on the blackboard for other nodes to use.
7+
8+
Input Ports
9+
-----------
10+
11+
:robot_base_frame:
12+
13+
=============================== =======
14+
Type Default
15+
------------------------------- -------
16+
string N/A
17+
=============================== =======
18+
19+
Description
20+
Robot base frame to transform poses to if not given in the same frame. If not provided, uses the BT Navigator's ``base_frame`` setting automatically.
21+
22+
:global_frame:
23+
24+
====== =======
25+
Type Default
26+
------ -------
27+
string N/A
28+
====== =======
29+
30+
Description
31+
Global frame to transform poses to if not given in the same frame. If not provided, uses the BT Navigator's ``global_frame`` setting automatically.
32+
33+
Output Ports
34+
------------
35+
36+
:current_pose:
37+
38+
=============================== =======
39+
Type Default
40+
------------------------------- -------
41+
geometry_msgs::msg::PoseStamped N/A
42+
=============================== =======
43+
44+
Description
45+
The current pose in the global frame.
46+
47+
Example
48+
-------
49+
50+
.. code-block:: xml
51+
52+
<GetCurrentPose current_pose="{current_pose}"/>

‎configuration/packages/bt-plugins/actions/GetNextFewGoals.rst‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
.. _bt_get_next_few_goals_action:
2+
3+
GetNextFewGoals
4+
===============
5+
6+
Extracts only the next ``N`` goals from a list of goals to send to a later task that only needs localized future knowledge.
7+
8+
Input Ports
9+
-----------
10+
11+
:num_goals:
12+
13+
=============================== =======
14+
Type Default
15+
------------------------------- -------
16+
int N/A
17+
=============================== =======
18+
19+
Description
20+
How many of the goals to take from the input goals.
21+
22+
:input_goals:
23+
24+
=============================== =======
25+
Type Default
26+
------------------------------- -------
27+
nav_msgs/Goals N/A
28+
=============================== =======
29+
30+
Description
31+
Input goals list.
32+
33+
34+
Output Ports
35+
------------
36+
37+
:output_goals:
38+
39+
=============================== =======
40+
Type Default
41+
------------------------------- -------
42+
nav_msgs/Goals N/A
43+
=============================== =======
44+
45+
Description
46+
The output pruned goals list.
47+
48+
Example
49+
-------
50+
51+
.. code-block:: xml
52+
53+
<GetNextFewGoals num_goals="3" input_goals="{goal_poses}" output_goals="{planning_goals}"/>

0 commit comments

Comments
 (0)