Backports to iron (#73)

* Fix polygons not clearing (#72) * Add behavior tree to navigate to start before starting fields navigation * snake case * comments * rviz visualization for demo * Update rviz_config.rviz * Linting and add to other demo * Fix polygons not clearing, credit to tonynajjar --------- Co-authored-by: Marc Morcos <[email protected]> * Rviz Visualization Config (#67) * Add behavior tree to navigate to start before starting fields navigation * snake case * comments * rviz visualization for demo * Update rviz_config.rviz * Linting and add to other demo * Add behavior tree to navigate to start before starting fields navigation (#65) * Add behavior tree to navigate to start before starting fields navigation * snake case * comments * Old format for behavior tree --------- Co-authored-by: Marc Morcos <[email protected]>
open-navigation · Aug 19, 2024 · 6ff556f · 6ff556f
1 parent 48ba3e7
commit 6ff556f
Show file tree

Hide file tree

Showing 5 changed files with 731 additions and 2 deletions.
diff --git a/opennav_coverage_bt/behavior_trees/navigate_w_basic_complete_coverage_nav_to_start.xml b/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 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>
diff --git a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp
@@ -46,6 +46,7 @@ void ComputeCoveragePathAction::on_tick()
     // Convert from vector of Polygons to coverage sp. message
     std::vector<geometry_msgs::msg::Polygon> polys;
     getInput("polygons", polys);
+    goal_.polygons.clear();
     goal_.polygons.resize(polys.size());
     for (unsigned int i = 0; i != polys.size(); i++) {
       for (unsigned int j = 0; j != polys[i].points.size(); j++) {

diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py
@@ -25,6 +25,7 @@
 def generate_launch_description():
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     coverage_demo_dir = get_package_share_directory('opennav_coverage_demo')
+    rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz')
 
     world = os.path.join(coverage_demo_dir, 'blank.world')
     param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml')
@@ -66,7 +67,7 @@ def generate_launch_description():
     rviz_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
-        launch_arguments={'namespace': ''}.items())
+        launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items())
 
     # start navigation
     bringup_cmd = IncludeLaunchDescription(

diff --git a/opennav_coverage_demo/launch/row_coverage_demo_launch.py b/opennav_coverage_demo/launch/row_coverage_demo_launch.py
@@ -25,6 +25,7 @@
 def generate_launch_description():
     nav2_bringup_dir = get_package_share_directory('nav2_bringup')
     coverage_demo_dir = get_package_share_directory('opennav_coverage_demo')
+    rviz_config_file = os.path.join(coverage_demo_dir, 'rviz_config.rviz')
 
     world = os.path.join(coverage_demo_dir, 'blank.world')
     param_file_path = os.path.join(coverage_demo_dir, 'demo_params.yaml')
@@ -66,7 +67,7 @@ def generate_launch_description():
     rviz_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py')),
-        launch_arguments={'namespace': ''}.items())
+        launch_arguments={'namespace': '', 'rviz_config': rviz_config_file}.items())
 
     # start navigation
     bringup_cmd = IncludeLaunchDescription(