Add BehaviorTree visualization support using Groot (#1725)

* Add BehaviorTree visualization support using Groot

* Add nav2 TreeNodesModel containing all BT Nodes used

* Add instructions on using Groot to visualize behavior trees

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Rearrange files and minor updates

* Move nav2_tree_nodes.xml and groot instruction to nav2_beahvior_tree

* Run cmake install rules for nav2_tree_nodes.xml

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Add distance controller node in nav2 TreeNodesModel

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

nav2_behavior_tree/CMakeLists.txt

@@ -123,6 +123,8 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(FILES nav2_tree_nodes.xml DESTINATION share/${PROJECT_NAME})
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()

nav2_behavior_tree/groot_instructions.md

@@ -0,0 +1,12 @@
+# Instructions on using Groot
+[Groot](https://github.com/BehaviorTree/Groot) is the companion application of [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP) to create, edit, and monitor behavior trees.
+
+##### Note: Currently fully supports visualization of the behavior trees. It also supports the creation of custom nodes except control flow nodes. Support for custom control flow nodes and real-time monitoring is under development.
+
+### BehaviorTree visualization
+To visualize the behavior trees using Groot:
+1. Open Groot in editor mode
+2. Select the `Load palette from file` option  (import button) near the top left corner.
+3. Open the file `/path/to/navigation2/nav2_behavior_tree/nav2_tree_nodes.xml` to import all the behavior tree nodes used for navigation.
+4. Select `Load tree` option near the top left corner
+5. Browse the tree you want to visualize the select ok.

nav2_behavior_tree/nav2_tree_nodes.xml

@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<!--
+  For instructions on using Groot and description of the following BehaviorTree nodes,
+  please refer to the groot_instructions.md and REAMDE.md respectively located in the 
+  nav2_behavior_tree package.
+-->
+<root>
+  <TreeNodesModel>
+    <!-- ############################### ACTION NODES ################################# -->
+    <Action ID="BackUp">
+      <input_port name="backup_dist">Distance to backup</input_port>
+      <input_port name="backup_speed">Speed at which to backup</input_port>
+    </Action>
+
+    <Action ID="ClearEntireCostmap">
+      <input_port name="service_name">Service name</input_port>
+    </Action>
+
+    <Action ID="ComputePathToPose">
+      <input_port name="goal">Destination to plan to</input_port>
+      <output_port name="path">Path created by ComputePathToPose node</output_port>
+      <input_port name="planner_id"/>
+    </Action>
+
+    <Action ID="FollowPath">
+      <input_port name="controller_id" default="FollowPath"/>
+      <input_port name="path">Path to follow</input_port>
+    </Action>
+
+    <Action ID="NavigateToPose">
+        <input_port name="orientation">Orientation</input_port>
+        <input_port name="position">Position</input_port>
+    </Action>
+
+    <Action ID="RandomCrawl"/>
+
+    <Action ID="Spin">
+      <input_port name="spin_dist">Spin distance</input_port>
+    </Action>
+
+    <Action ID="Wait">
+      <input_port name="wait_duration">Wait time</input_port>
+    </Action>
+
+    <!-- ############################### CONDITION NODES ############################## -->
+    <Condition ID="GoalReached">
+        <input_port name="goal">Destination</input_port>
+    </Condition>
+
+    <Condition ID="IsStuck"/>
+
+    <Condition ID="TransformAvailable">
+      <input_port name="child">Child frame for transform</input_port>
+      <input_port name="parent">Parent frame for transform</input_port>
+    </Condition>
+
+    <!-- ############################### CONTROL NODES ################################ -->
+    <Control ID="PipelineSequence"/>
+
+    <Control ID="RecoveryNode">
+      <input_port name="number_of_retries">Number of retries</input_port>
+    </Control>
+
+    <Control ID="RoundRobin"/>
+
+    <!-- ############################### DECORATOR NODES ############################## -->
+    <Decorator ID="RateController">
+      <input_port name="hz">Rate</input_port>
+    </Decorator>
+
+    <Decorator ID="DistanceController">
+      <input_port name="distance">Distance</input_port>
+    </Decorator>
+
+  </TreeNodesModel>
+</root>