🌳 Waypoint Navigation BT
As mentioned earlier, some Waypoint Navigation implementations support using Behavior Trees to define
the logic and the recoveries for task execution.
In this section, you will learn about the available Behavior Trees, their characteristics, and how
to create new ones with new custom logic.
In addition to the Nav2 Behavior Trees,
PAL robots are equipped with the following behavior trees for waypoint navigation:
Note
You can list the available Nav2 Behavior Trees with
ls /opt/ros/humble/share/nav2_bt_navigator/behavior_trees/
And the available PAL Behavior Trees with
ls /opt/pal/alum/share/pal_nav2_bt_navigator/behavior_trees/
In addition to the
Nav2 Behavior Trees Nodes,
PAL robots are equipped with the following behavior tree nodes for waypoint navigation:
To learn more about the types of BT nodes, you can refer to the official
BT basic documentation.
Behavior Trees
Follow waypoints
This Behavior Tree implements a Waypoint Navigation logic that attempts to reach each waypoint only
once, in the specified order.
If the navigation to any waypoint fails, the waypoint navigation task is aborted and the
subsequent waypoints are skipped.
Follow waypoints with process
This Behavior Tree implements a Waypoint Navigation logic that attempts to reach each waypoint
only once, in the specified order.
Furthermore, upon reaching each Waypoint, this Behavior Tree executes one or more actions
implemented as
Waypoint Task Executor plugins.
If the navigation to any waypoint fails, the waypoint navigation task is aborted and the subsequent
waypoints are skipped.
Behavior Tree Nodes
GetNextGoal
Checks whether the current goal has been achieved.
If so updates the Blackboard and sets the next goal and the next actions.
|
Port Name |
Type |
Description |
|---|
Input Ports |
goals
|
(std::vector<pal_nav2_msgs::msg::Waypoint>, DEFAULT=N/A)
|
List of goals the robot should Navigate to. |
goal_achieved
|
(bool, DEFAULT=false)
|
Whether or not the current goal has
been achieved by the Navigation system. |
Output Ports |
goal_achieved
|
(bool, DEFAULT=false)
|
Whether or not the current goal has
been achieved by the Navigation system. |
goal
|
(geometry_msgs::msg::PoseStamped, DEFAULT=N/A)
|
Next goal to reach. |
actions
|
(std::vector<pal_nav2_msgs::msg::Action>, DEFAULT=N/A)
|
Next actions to perform upon reaching the
goal. |
ProcessAtWaypoint
Implements a BT::SyncActionNode that offers the flexibility to
define what to do upon reaching each Waypoint, in the form of an Action.
This Action Node of the BT allows to dynamically load any WaypointTaskExecutor
plugins defining the logic and the action that the robot will execute upon reaching the Waypoint.
This node can load any existent WaypointTaskExecutor plugin as well as any new user-defined plugin.
|
Name |
Type |
Description |
|---|
Parameters |
process_at_waypoint_plugins
|
(std::vector<pal_nav2_msgs::msg::Waypoint>, DEFAULT=N/A)
|
List of the supported WaypointTaskExecutor plugins.
Listing a plugin here doesn’t mean it will be
automatically loaded! It will be loaded only when
needed, i.e. when a Waypoint defines an Action that
uses this plugin. |
<PLUGIN-NAME>.package
|
(bool, DEFAULT=false)
|
Name of the package that contains the plugin.
As the loader is able to load any plugin
inheriting from WaypointTaskExecutor you need
to specify where your plugin is. |
<PLUGIN-NAME>.plugin
|
(string, REQUIRED)
|
Name of the plugin itself.
This should correspond to the name you used
to export it. (ex.
plugin: "nav2_waypoint_follower::WaitAtWaypoint"). |
Input Ports |
curr_pose
|
(geometry_msgs::msg::PoseStamped, DEFAULT=N/A)
|
Current robot pose. |
actions
|
(std::vector<pal_nav2_msgs::msg::Action>, DEFAULT=N/A)
|
List of actions to perform upon reaching the goal. |
SetNavConfigurations
Sets a navigation configuration for a selected node.
|
Port Name |
Type |
Description |
|---|
Input Ports |
node_name
|
(string, DEFAULT=N/A)
|
Selected node name. |
configs
|
(vector, DEFAULT=N/A)
|
Configurations files name. |
SetParameters
Sets a parameter for a selected node
|
Port Name |
Type |
Description |
|---|
Input Ports |
node_name
|
(string, DEFAULT=N/A)
|
The name of the node whose parameter is to be set. |
parameters
|
([(param_type, param_name, param_value),...], DEFAULT=N/A)
|
The parameters to be set. |
AllGoalsAchieved
If the last goal in a list of goals has been achieved, it returns SUCCESS.
|
Port Name |
Type |
Description |
|---|
Input Ports |
goal_achieved
|
(bool, DEFAULT=false)
|
Whether or not the last goal has been achieved. |
How to create your Waypoint Navigation Behavior Tree Node
In addition to all the PAL Behavior Tree nodes and the Nav2 Behavior Tree, you might want to develop
your custom node implementing a desired behavior.
Note
To see the list of all the PAL Behavior Tree nodes, you can use the command:
cat /opt/pal/alum/share/pal_nav2_behavior_tree/pal_nav2_tree_nodes.xml
To see the list of all the Nav2 Behavior Tree nodes, you can use the command:
cat /opt/ros/humble/share/nav2_behavior_tree/nav2_tree_nodes.xml
For example, you might want to develop a new recovery behavior for the robot to adopt when a
certain Waypoint is not reachable. In this case, some desired behaviors can be:
Abort all waypoints
Skip to the next waypoint
Retry the current waypoint
Go back to the previous waypoint
To create your Behavior Tree node with your custom logic, you can refer to the
Nav2 official documentation.
Once created, you can start using your Behavior Tree node for the Waypoint Navigation by modifying the
corresponding configuration file.
To use your node with the Nav2 NavigateThroughPoses you can create a new
configuration file and add your BT node in it.
mkdir ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator
touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_navigate_w_replanning_and_recovery.yaml
To configure the parameters file follow the guide in the 📔 Navigation Configuration Guide section
and then add a new entry to the plugin_lib_names list parameter:
sudo vi ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_navigate_w_replanning_and_recovery.yaml
__node_name__:
ros__parameters:
# NavigateThroughPoses parameters
plugin_lib_names:
- # NavigateThroughPoses BT plugins
- your_bt_node
Finally, to apply the changes, restart the navigation with the command:
pal module restart navigation
Otherwise, if you want to use your custom BT node with the PAL NavigateThroughWaypoints, you should create the corresponding configuration file
and add your BT node in it.
touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_navigate_through_waypoints.yaml
To configure the parameters file follow the guide in the 📔 Navigation Configuration Guide section
and then add a new entry to the plugin_lib_names list parameter:
__node_name__:
ros__parameters:
# NavigateThroughWaypoints parameters
plugin_lib_names:
- # NavigateThroughWaypoints BT plugins
- your_bt_node
Finally, to apply the changes, restart the advanced navigation with the command:
pal module restart advanced_navigation
How to create your Waypoint Navigation Behavior Tree
In addition to the PAL Behavior Trees and the Nav2 Behavior Trees,
you can create a new Waypoint Navigation Behavior Tree with custom logic, by creating a new XML file.
To do so, you can refer to the
BT Official Documentation and
configure it by adding your nodes to the XML file as indicated in the
Official Configuration Guide.
You can create your custom XML file in your own folder in the ~/.pal directory of your robot:
mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/behavior_trees
And then, within this folder, you can create a new XML file for your custom Behavior Tree.
touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/behavior_trees/my_custom_bt.xml
Once created, you can start using your XML file with your custom BT with any of the Waypoint Navigation
implementations that support Behavior Trees.
To use your node with the Nav2 NavigateThroughPoses you can directly use the
Nav2 NavigateThroughPoses Action
and provide the full path to your XML file in the behavior_tree field of the goal message.
To use your node with the PAL NavigateThroughWaypoints
you can directly use the PAL NavigateThroughWaypoints Action and provide the full path to your XML file
in the behavior_tree field of the goal message.
Note
To visualize the PAL NavigateThroughWaypoints Action interface you can use the command:
ros2 interface show pal_nav2_msgs/action/NavigateThroughWaypoints
Otherwise, if you want to set your custom BT as the default one for the PAL NavigateThroughWaypoints, you can use the following command:
sudo vi ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_navigate_through_waypoints.yaml
To change the default_nav_through_waypoints_bt_xml parameter with the full path to your XML file
__node_name__:
ros__parameters:
# NavigateThroughWaypoints parameters
default_nav_through_waypoints_bt_xml: "/home/pal/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/behavior_trees/my_custom_bt.xml"
Finally, to apply the changes, restart the advanced navigation with the command:
pal module restart advanced_navigation
