Added documentation for Groot 2 (#672)

* Doc for Groot2

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor fixes

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Minor fixes

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

* Update plugin_tutorials/docs/writing_new_bt_plugin.rst

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Signed-off-by: Alberto Tudela <ajtudela@gmail.com>

---------

Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Author: ajtudela

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
+*************************
+
+:``<navigate_to_pose_name>``.enable_groot_monitoring:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  bool           False
+  ============== =======
+
+  Description
+    Whether to enable Groot2 monitoring for this navigator.
+
+:``<navigate_to_pose_name>``.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
+*******************************
+
+:``<navigate_through_poses>``.enable_groot_monitoring:
+
+  ============== =======
+  Type           Default
+  -------------- -------
+  bool           False
+  ============== =======
+
+  Description
+    Whether to enable Groot2 monitoring for this navigator.
+
+:``<navigate_through_poses>``.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

migration/Jazzy.rst

@@ -429,3 +429,9 @@ 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
+***************
+In `PR #5065 <https://github.com/ros-navigation/navigation2/pull/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.
+
+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.

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,84 @@ Select this BT XML file in your specific navigation request in ``NavigateToPose`
       </RecoveryNode>
     </BehaviorTree>
   </root>
+
+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<geometry_msgs::msg::Point>("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<double>(parts[0]);
+      position.y = BT::convertFromString<double>(parts[1]);
+      position.z = BT::convertFromString<double>(parts[2]);
+      return position;
+    }
+  }
+  }  // namespace BT
+
+For more information on custom type conversion, you can refer to the `bt_utils.hpp <https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/include/nav2_behavior_tree/bt_utils.hpp>`_ or the BT.CPP documentation: `Parsing a string <https://www.behaviortree.dev/docs/tutorial-basics/tutorial_03_generic_ports#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<geometry_msgs::msg::Point>();
+    return providedBasicPorts(
+      BT::OutputPort<geometry_msgs::msg::Point>("position", "Position of the robot")
+    });
+  }
+
+For more information on custom type conversion, you can refer to the `json_utils.hpp <https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/include/nav2_behavior_tree/json_utils.hpp>`_ or the BT.CPP documentation: `Visualize custom types in the Blackboard <https://www.behaviortree.dev/docs/tutorial-basics/tutorial_11_groot2/#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.