🌳 Waypoint Navigation BT#

Attention

Follow this chapter only if you are willing to use a BT-based Waypoint Navigation implementation such as Nav2 NavigateThroughPoses or PAL NavigateThroughWaypoints.

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:

follow_waypoints.xml
follow_waypoints_w_process.xml

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:

Plugin Name	BT Node Type
GetNextGoal	Action
ProcessAtWaypoint	Action
SetNavConfigurations	Action
SetParameters	Action
AllGoalsAchieved	Condition

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.

../_images/follow_waypoints_w_process.svg

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.
Input Ports	`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.
Input Ports	`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.
Input Ports	`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.
Input Ports	`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. To do so, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/config

And then, within this folder, you can create a new navigate_through_waypoints configuration file.

touch ~/.pal/config/99_my_navigate_through_waypoints_config.yaml

Attention

The name of the file should start with a number to ensure that it is loaded last and overrides the default PAL configurations.

Within the newly created 99_my_navigate_through_waypoints_config.yaml file, you can insert your navigate_through_waypoints parameters. 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:

/navigate_through_waypoints:
   ros__parameters:
      # Your navigate_through_waypoints parameters

      plugin_lib_names:
       - nav2_navigate_to_pose_action_bt_node
       - pal_nav2_get_next_goal_action_bt_node
       - pal_nav2_process_at_waypoint_action_bt_node
       - pal_nav2_all_goals_achieved_condition_bt_node
       - your_bt_node

Once created, to start using your custom navigate_through_waypoints configuration, you need to restart the advanced_navigation module with the command:

pal module restart advanced_navigation

Note

This change is is persistent and will be loaded every time you start the advanced_navigation module. If you want to revert to the default PAL configuration, you can simply delete the custom configuration file you created in the ~/.pal/config folder.

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

You can create it in the directory you prefer, though it is recommended to create it in the .pal folder of your robot.

mkdir -p ~/.pal/behavior_trees

And then, within this folder, you can create a new XML file for your custom Behavior Tree.

touch ~/.pal/behavior_trees/my_custom_bt.xml

In order to start using your custom Behavior Tree, you can :

Use the /navigate_through_waypoints by providing the full path to your custom XML file as shown in the previous section 🚩 Waypoint Navigation.

ros2 action send_goal /navigate_through_waypoints pal_nav2_msgs/action/NavigateThroughWaypoints "waypoints: []
behavior_tree: '/home/pal/.pal/behavior_trees/my_custom_bt.xml'
number_of_loops: 0"

Create your custom configuration for the navigate_through_waypoints and update the default default_nav_through_waypoints_bt_xml parameter to the full path of your custom XML file.

If you want to go with the second option, you first need to create a new directory in the .pal folder of your robot.

mkdir -p ~/.pal/config

And then, within this folder, you can create a new navigate_through_waypoints configuration file.

touch ~/.pal/config/99_my_navigate_through_waypoints_config.yaml

Attention

The name of the file should start with a number to ensure that it is loaded last and overrides the default PAL configurations.

Within the newly created 99_my_navigate_through_waypoints_config.yaml file, you can insert your navigate_through_waypoints parameters.

/navigate_through_waypoints:
   ros__parameters:
      # Your navigate_through_waypoints parameters

      default_nav_through_waypoints_bt_xml: "/home/pal/.pal/behavior_trees/my_custom_bt.xml"

Once created, to start using your custom navigate_through_waypoints configuration, you need to restart the advanced_navigation module with the command:

pal module restart advanced_navigation

Note