Add behavior tree to navigate to start before starting fields navigat…

…ion (#65)

* Add behavior tree to navigate to start before starting fields navigation

* snake case

* comments

opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml

@@ -0,0 +1,35 @@
+<!--
+  This Behavior Tree computes a complete coverage path over a selected space, navigates to the start point of the path, then follows the coverage path.
+  Can also take the coverage_path instead of Nav Path for segmented swaths from turns
+  for changes in operational modes (e.g. raise tool, turn off payload in turns not in field).
+
+  Field filepath is set by the Navigator plugin in the action request, but also may be hardcoded
+  or set using the polygons input port instead for dynamically computed values instead of from file.
+
+  It might be wise to still have the planner server on hand to connect to the coverage path in realistic uses.
+
+  This BT shows set polygon usage with the coverage server
+-->
+
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RateController hz="0.0000001"> <!-- once, for demo -->
+      <Sequence name="NavigateWithoutReplanning">
+        <!-- May use:
+         'polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}"' if set polygon via NavigateCompleteCoverage
+         or file_field="{field_filepath}" if setting polygon file via NavigateCompleteCoverage -->
+
+         <!-- Compute path to cover fields -->
+        <ComputeCoveragePath nav_path="{path}" polygons="{field_polygon}" polygons_frame_id="{polygon_frame_id}" error_code_id="{compute_coverage_error_code}"/>
+
+        <!-- Go to start of path before navigating -->
+        <GetPoseFromPath path="{path}" pose="{start_pose}" index="0" />
+        <ComputePathToPose goal="{start_pose}" path="{path_to_start}" planner_id="GridBased" error_code_id="{compute_path_error_code}"/>
+        <FollowPath path="{path_to_start}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+
+        <!-- Follow computed path -->
+        <FollowPath path="{path}" controller_id="FollowPath" error_code_id="{follow_path_error_code}"/>
+      </Sequence>
+    </RateController>
+  </BehaviorTree>
+</root>