From 61be30bd5b9e9f286e78d0eba5f390d5a0674360 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Wed, 9 Apr 2025 20:29:01 +0200 Subject: [PATCH 1/4] Doc for Groot2 Signed-off-by: Alberto Tudela --- .../packages/configuring-bt-navigator.rst | 60 +++++++++++++- migration/Jazzy.rst | 5 ++ .../docs/writing_new_bt_plugin.rst | 82 +++++++++++++++++++ 3 files changed, 145 insertions(+), 2 deletions(-) diff --git a/configuration/packages/configuring-bt-navigator.rst b/configuration/packages/configuring-bt-navigator.rst index d502243563..8a4ff9d1fa 100644 --- a/configuration/packages/configuring-bt-navigator.rst +++ b/configuration/packages/configuring-bt-navigator.rst @@ -249,7 +249,8 @@ Parameters Description Use time provided by simulation. -:error_code_name_prefixes +:error_code_name_prefixes: + ============== =========================== Type Default -------------- --------------------------- @@ -270,7 +271,7 @@ Parameters Description For Kilted and newer: List of of error code name prefixes to be appended with '_error_code' and '_error_msg' and searched for during aborted navigator error processing. -:error_code_names +:error_code_names: ============== =========================== Type Default @@ -293,6 +294,57 @@ Parameters Description The lifecycle node bond mechanism publishing period (on the /bond topic). Disabled if inferior or equal to 0.0. + +NavigateToPose Parameters +************************* + +:````.enable_groot_monitoring: + + ============== ======= + Type Default + -------------- ------- + bool False + ============== ======= + + Description + Whether to enable Groot2 monitoring for this navigator. + +:````.groot_server_port: + + ==== ======= + Type Default + ---- ------- + int 1667 + ==== ======= + + Description + The port number for the Groot2 server. Note: In Groot2, you only need to specify the server port value, not the publisher port, as it is always the server port +1. Therefore, in this case, to use another navigator, the next available port would be 1669. + +NavigateThroughPoses Parameters +******************************* + +:````.enable_groot_monitoring: + + ============== ======= + Type Default + -------------- ------- + bool False + ============== ======= + + Description + Whether to enable Groot2 monitoring for this navigator. + +:````.groot_server_port: + + ==== ======= + Type Default + ---- ------- + int 1669 + ==== ======= + + Description + The port number for the Groot2 server. Note: In Groot2, you only need to specify the server port value, not the publisher port, as it is always the server port +1. Therefore, in this case, to use another navigator, the next available port would be 1671. + Example ******* .. code-block:: yaml @@ -315,8 +367,12 @@ Example navigators: ['navigate_to_pose', 'navigate_through_poses'] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::" + enable_groot_monitoring: false + groot_server_port: 1667 navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # In Iron and older versions, "/" was used instead of "::" + enable_groot_monitoring: false + groot_server_port: 1669 plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node - nav2_follow_path_action_bt_node diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index 2ddd8d09a4..ac6a3282a0 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -429,3 +429,8 @@ Modified statuses should then be propagated through output ports for downstream The ``NavigateThroughPoses`` navigator retrieves the ``waypoint_statuses`` instance from the blackboard in its ``onLoop`` callback and writes it into the feedback message. During the ``goalCompleted`` callback, it fetches the ``waypoint_statuses`` instance and, based on the BT's final execution status (``final_bt_status``), updates any waypoints still in the ``PENDING`` state to either ``COMPLETED`` (if ``final_bt_status`` is ``SUCCEEDED``) or ``FAILED`` (otherwise). + +Groot 2 Support +*************** + +BT navigators: ``navigate_to_pose`` and ``navigate_through_poses`` now support live monitoring and visualization of the behavior tree using Groot 2. JSON conversions are also available to see the content of the Blackboard, allowing the introspection of the BT nodes. Switching bt-xmls on the fly through a new goal request is also included. Because live monitoring of Behavior Tree with more than 20 nodes and visualizing the content of the blackboard is a PRO (paid) feature of Groot 2. This feature is disabled by default. diff --git a/plugin_tutorials/docs/writing_new_bt_plugin.rst b/plugin_tutorials/docs/writing_new_bt_plugin.rst index 9c9bdf5949..1a2a742339 100644 --- a/plugin_tutorials/docs/writing_new_bt_plugin.rst +++ b/plugin_tutorials/docs/writing_new_bt_plugin.rst @@ -6,6 +6,8 @@ Writing a New Behavior Tree Plugin - `Overview`_ - `Requirements`_ - `Tutorial Steps`_ +- `Using custom types for Input/Output ports`_ +- `Visualize the content of the blackboard in Groot 2 (PRO)`_ Overview ======== @@ -235,3 +237,83 @@ Select this BT XML file in your specific navigation request in ``NavigateToPose` + +Using custom types for Input/Output ports +========================================= + +In addition to standard types, custom types such as those from `nav2_msgs` or `geometry_msgs` can also be used for input/output ports. + +For example, you can define custom types for ports in the `providedPorts` function as follows: + +.. code-block:: cpp + + static PortsList providedPorts() + { + return providedBasicPorts( + BT::OutputPort("position", "Position of the robot") + }); + } + +To use custom types for input/output ports in the behavior tree XML, it is necessary to convert them from a string. This is because ports in the XML are represented as strings and must be transformed into the corresponding data type in the code. + +For example, if you have a custom type `geometry_msgs::msg::Point`, you can perform the conversion as follows: + +.. code-block:: cpp + + namespace BT + { + template<> + inline geometry_msgs::msg::Point convertFromString(const StringView key) + { + // three real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 3) { + throw std::runtime_error("invalid number of fields for point attribute)"); + } else { + geometry_msgs::msg::Point position; + position.x = BT::convertFromString(parts[0]); + position.y = BT::convertFromString(parts[1]); + position.z = BT::convertFromString(parts[2]); + return position; + } + } + } // namespace BT + +For more information on custom type conversion, you can refer to the `bt_utils.hpp `_ or the BT.CPP documentation : `Parsing a string `_ + +Visualize the content of the blackboard in Groot 2 (PRO) +======================================================== +If you are using the paid version of Groot 2 Pro, you can view the content of the blackboard of the BT nodes. To achieve this, you need to perform a conversion of the custom input/output type to JSON format. Below is an example of such a conversion: + +.. code-block:: cpp + + namespace geometry_msgs::msg + { + BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg) + { + add_field("x", &msg.x); + add_field("y", &msg.y); + add_field("z", &msg.z); + } + } // namespace geometry_msgs::msg + +The macro ``BT_JSON_CONVERTER`` must be placed within the namespace of the custom type to be converted. Additionally, if the custom type is composed of other custom types, those types must be converted first before converting the parent type. + +After defining the conversion, you need to register the custom type in the `providedPorts` function. Below is an example of how to register it: + +.. code-block:: cpp + + static PortsList providedPorts() + { + // Register JSON definitions for the types used in the ports + BT::RegisterJsonDefinition(); + return providedBasicPorts( + BT::OutputPort("position", "Position of the robot") + }); + } + +For more information on custom type conversion, you can refer to the `json_utils.hpp `_ or the BT.CPP documentation : `Visualize custom types in the Blackboard `_ + + .. note:: + + All custom types used in Nav2 are already registered in the `json_utils.hpp` file. You can use them directly without the need to register them again. \ No newline at end of file From da7175e9f4fb91ff36cc9d2e936d3c707971d308 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 10 Apr 2025 14:43:20 +0200 Subject: [PATCH 2/4] Minor fixes Signed-off-by: Alberto Tudela --- migration/Jazzy.rst | 3 ++- plugin_tutorials/docs/writing_new_bt_plugin.rst | 10 +++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/migration/Jazzy.rst b/migration/Jazzy.rst index ac6a3282a0..28b5cd30f8 100644 --- a/migration/Jazzy.rst +++ b/migration/Jazzy.rst @@ -432,5 +432,6 @@ During the ``goalCompleted`` callback, it fetches the ``waypoint_statuses`` inst Groot 2 Support *************** +In `PR #5065 `_ , BT navigators: ``navigate_to_pose`` and ``navigate_through_poses`` now support live monitoring and visualization of the behavior tree using Groot 2. JSON conversions are also available to see the content of the Blackboard, allowing the introspection of the BT nodes. Switching bt-xmls on the fly through a new goal request is also included. -BT navigators: ``navigate_to_pose`` and ``navigate_through_poses`` now support live monitoring and visualization of the behavior tree using Groot 2. JSON conversions are also available to see the content of the Blackboard, allowing the introspection of the BT nodes. Switching bt-xmls on the fly through a new goal request is also included. Because live monitoring of Behavior Tree with more than 20 nodes and visualizing the content of the blackboard is a PRO (paid) feature of Groot 2. This feature is disabled by default. +Because live monitoring of Behavior Tree with more than 20 nodes and visualizing the content of the blackboard is a PRO (paid) feature of Groot 2. This feature is disabled by default. diff --git a/plugin_tutorials/docs/writing_new_bt_plugin.rst b/plugin_tutorials/docs/writing_new_bt_plugin.rst index 1a2a742339..6e79550a6f 100644 --- a/plugin_tutorials/docs/writing_new_bt_plugin.rst +++ b/plugin_tutorials/docs/writing_new_bt_plugin.rst @@ -241,9 +241,9 @@ Select this BT XML file in your specific navigation request in ``NavigateToPose` Using custom types for Input/Output ports ========================================= -In addition to standard types, custom types such as those from `nav2_msgs` or `geometry_msgs` can also be used for input/output ports. +In addition to standard types, custom types such as those from ``nav2_msgs`` or ``geometry_msgs`` can also be used for input/output ports. -For example, you can define custom types for ports in the `providedPorts` function as follows: +For example, you can define custom types for ports in the ``providedPorts`` function as follows: .. code-block:: cpp @@ -256,7 +256,7 @@ For example, you can define custom types for ports in the `providedPorts` functi To use custom types for input/output ports in the behavior tree XML, it is necessary to convert them from a string. This is because ports in the XML are represented as strings and must be transformed into the corresponding data type in the code. -For example, if you have a custom type `geometry_msgs::msg::Point`, you can perform the conversion as follows: +For example, if you have a custom type ``geometry_msgs::msg::Point``, you can perform the conversion as follows: .. code-block:: cpp @@ -279,7 +279,7 @@ For example, if you have a custom type `geometry_msgs::msg::Point`, you can perf } } // namespace BT -For more information on custom type conversion, you can refer to the `bt_utils.hpp `_ or the BT.CPP documentation : `Parsing a string `_ +For more information on custom type conversion, you can refer to the `bt_utils.hpp `_ or the BT.CPP documentation: `Parsing a string `_. Visualize the content of the blackboard in Groot 2 (PRO) ======================================================== @@ -312,7 +312,7 @@ After defining the conversion, you need to register the custom type in the `prov }); } -For more information on custom type conversion, you can refer to the `json_utils.hpp `_ or the BT.CPP documentation : `Visualize custom types in the Blackboard `_ +For more information on custom type conversion, you can refer to the `json_utils.hpp `_ or the BT.CPP documentation: `Visualize custom types in the Blackboard `_ .. note:: From 1a1e0e9d2d069b99c948d62694994a4df93a1243 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 10 Apr 2025 16:29:12 +0200 Subject: [PATCH 3/4] Minor fixes Signed-off-by: Alberto Tudela --- plugin_tutorials/docs/writing_new_bt_plugin.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugin_tutorials/docs/writing_new_bt_plugin.rst b/plugin_tutorials/docs/writing_new_bt_plugin.rst index 6e79550a6f..0c857345d4 100644 --- a/plugin_tutorials/docs/writing_new_bt_plugin.rst +++ b/plugin_tutorials/docs/writing_new_bt_plugin.rst @@ -241,7 +241,7 @@ Select this BT XML file in your specific navigation request in ``NavigateToPose` Using custom types for Input/Output ports ========================================= -In addition to standard types, custom types such as those from ``nav2_msgs`` or ``geometry_msgs`` can also be used for input/output ports. +In addition to standard types, custom types such as those from ``nav2_msgs`` or ``geometry_msgs`` can also be used for input/output ports. For example, you can define custom types for ports in the ``providedPorts`` function as follows: @@ -316,4 +316,4 @@ For more information on custom type conversion, you can refer to the `json_utils .. note:: - All custom types used in Nav2 are already registered in the `json_utils.hpp` file. You can use them directly without the need to register them again. \ No newline at end of file + All custom types used in Nav2 are already registered in the `json_utils.hpp` file. You can use them directly without the need to register them again. From 484ead821be9d3a93069850f02297e3c7944f797 Mon Sep 17 00:00:00 2001 From: Alberto Tudela Date: Thu, 10 Apr 2025 19:44:00 +0200 Subject: [PATCH 4/4] Update plugin_tutorials/docs/writing_new_bt_plugin.rst Signed-off-by: Alberto Tudela Co-authored-by: Steve Macenski Signed-off-by: Alberto Tudela --- plugin_tutorials/docs/writing_new_bt_plugin.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/plugin_tutorials/docs/writing_new_bt_plugin.rst b/plugin_tutorials/docs/writing_new_bt_plugin.rst index 0c857345d4..c52799e539 100644 --- a/plugin_tutorials/docs/writing_new_bt_plugin.rst +++ b/plugin_tutorials/docs/writing_new_bt_plugin.rst @@ -283,6 +283,7 @@ For more information on custom type conversion, you can refer to the `bt_utils.h Visualize the content of the blackboard in Groot 2 (PRO) ======================================================== + If you are using the paid version of Groot 2 Pro, you can view the content of the blackboard of the BT nodes. To achieve this, you need to perform a conversion of the custom input/output type to JSON format. Below is an example of such a conversion: .. code-block:: cpp