🌳 Target Navigation BT#

As mentioned in 🎯 Target Navigation chapter, the Target Navigation framework is implemented in the NavigateToTarget Navigator which is a Nav2 Behavior-Tree Navigator. As such, this navigator allows you to define a custom navigation logic using a Behavior Tree to define the robot’s behavior while approaching a target and any recovery logics.

In this section, you will find the available Behavior Trees for the Target Navigation module and learn how to create your own.

PAL robots are equipped with the following behavior tree for the target navigation:

navigate_to_target.xml

Note

You can find the available PAL behavior tree in

ls /opt/pal/${PAL_DISTRO}/share/pal_nav2_bt_navigator/behavior_trees/

This behavior tree is used to navigate the robot to a target and has the following nodes:

Plugin Name	BT Node Type
DetectTarget	Action
NavigateToPose	Action

To learn more about the types of BT nodes, you can refer to the official BT basic documentation.

Behavior Tree#

Navigate To Target#

The main BT for the target navigation is the navigate_to_target.xml. Using this BT allows for dynamic adjustment of the navigation strategy based on the specific requirements of the target, the detector’s output, and the current environmental context.

The functionality of the BT is to detect a target pose and process it with the NavigateToPose.

Behavior Tree Nodes#

DetectTarget#

Detects and localizes a target in the environment.

	Port Name	Type	Description
Input Ports	`target`	`(int, DEFAULT=N/A)`	Navigation target identifier.
Input Ports	`detector`	`(string, DEFAULT=N/A)`	Detector used for detecting the target pose.
Output Ports	`goal`	`(geometry_msgs::msg::PoseStamped, DEFAULT=N/A)`	Target pose.
Output Ports	`error_code_id`	`(uint16, DEFAULT=N/A)`	Error code in case an error has occurred.

NavigateToPose#

Navigates the robot to a target identified by an ID, using a specified detector and a Behavior Tree (BT) for the navigation logic. This navigator extends the functionalities provided by the NAV2 official documentation.

	Port Name	Type	Description
Input Ports	`target`	`(int, DEFAULT=N/A)`	Navigation target identifier.
	`detector`	`(string, DEFAULT=N/A)`	Detector used for detecting the target pose.
	`behavior_tree`	`(string, DEFAULT=N/A)`	BT used for navigating to the target pose.
Output Ports	`error_code_id`	`(uint16, DEFAULT=N/A)`	Error code in case an error has occurred.

Usage#

To configure this BT node you can refer to the 🔌 Target Navigation API section.

To use the NavigateToTarget Navigator you need to send a goal to the action server /navigate_to_target, specifying the target ID, the detector name and a BT for the navigation logic.

How to create your Target 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/${PAL_DISTRO}/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/${ROS_DISTRO}/share/nav2_behavior_tree/nav2_tree_nodes.xml

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 Target Navigation by creating a new configuration file in the ~/.pal directory of your robot:

mkdir -p ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator

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

touch ~/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_bt_navigator_config.yaml

Within the newly created my_bt_navigator_config.yaml file, you can insert your custom bt_navigator parameters. For the list of available parameters, you can refer to the Nav2 BT Navigator configuration guide. When creating a new Navigation configuration file, apart from the node parameters, you also need to specify the robot to which they refer.

pal_navigation_cfg:
  ros__parameters:
      supported_robots:
      - some_pal_robot # e.g. tiago, omni_base, tiago_pro, etc.

bt_navigator:
  ros__parameters:
      # Your BT Navigator Parameters
      default_nav_to_pose_bt_xml: /home/pal/.pal/pal_navigation_cfg_params/params/nav2_bt_navigator/my_custom_bt_w_custom_plugin.xml # Full path to your custom BT XML file

      # Other parameters

      plugin_lib_names:
      # List of plugin libraries to load
      - my_bt_node

Once created, to start using your custom BT Navigator configuration, you need to set it in the navigation pipeline by modifying the <robot>_nav.yaml file in the <robot>_2dnav package.

sudo vi /opt/pal/$PAL_DISTRO/share/<robot>_2dnav/params/<robot>_nav.yaml

And then, change the params field of the bt_navigator to the name of your BT Navigator configuration file.

Finally, to apply the changes, restart the advanced navigation with the command:

pal module restart advanced_navigation

How to create your Target 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 the Target Navigation.

To use your BT with the NavigateToTarget Navigator you can directly use the action server /navigate_to_target and provide the full path to your XML file in the behavior_tree field of the goal message.

Otherwise, if you want to set your custom BT as the default one for the NavigateToTarget Navigator, you can use the following command:

sudo vi ~/.pal/pal_navigation_cfg_params/params/pal_nav2_target_detector/my_navigate_to_target.yaml

To change the default_nav_through_waypoints_bt_xml parameter with the full path to your XML file

__node_name__:
  ros__parameters:
    # NavigateToTargetNavigator parameters

    navigate_to_target:
      plugin: "pal_nav2_bt_navigator/NavigateToTargetNavigator"
    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