From 74c35ab8d268d066c17cde2922cb7e3699f721de Mon Sep 17 00:00:00 2001
From: TravisHC <745526203@qq.com>
Date: Mon, 9 Sep 2024 13:48:42 +0800
Subject: [PATCH 1/2] try adding inner obstacle

---
 opennav_coverage/CMakeLists.txt               | 24 +++++++++----------
 .../launch/coverage_demo_launch.py            |  2 +-
 .../opennav_coverage_demo/demo_coverage.py    | 18 +++++++++++---
 opennav_row_coverage/CMakeLists.txt           | 20 ++++++++--------
 4 files changed, 38 insertions(+), 26 deletions(-)

diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt
index 2e1ff4e9..58575dd3 100644
--- a/opennav_coverage/CMakeLists.txt
+++ b/opennav_coverage/CMakeLists.txt
@@ -85,18 +85,18 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-install(FILES test/test_field.xml
-              test/cartesian_test_field.xml
-              test/irregular_test_field.xml
-  DESTINATION share/${PROJECT_NAME}/
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
-endif()
+# install(FILES test/test_field.xml
+#               test/cartesian_test_field.xml
+#               test/irregular_test_field.xml
+#   DESTINATION share/${PROJECT_NAME}/
+# )
+
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   find_package(ament_cmake_gtest REQUIRED)
+#   ament_lint_auto_find_test_dependencies()
+#   add_subdirectory(test)
+# endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})
diff --git a/opennav_coverage_demo/launch/coverage_demo_launch.py b/opennav_coverage_demo/launch/coverage_demo_launch.py
index 1db1a335..38de1dbe 100644
--- a/opennav_coverage_demo/launch/coverage_demo_launch.py
+++ b/opennav_coverage_demo/launch/coverage_demo_launch.py
@@ -59,7 +59,7 @@ def generate_launch_description():
         arguments=[
             '-entity', 'tb3',
             '-file', sdf,
-            '-x', '5.0', '-y', '5.0', '-z', '0.10',
+            '-x', '0.5', '-y', '10.0', '-z', '0.10',
             '-R', '0.0', '-P', '0.0', '-Y', '0.0'])
 
     # start the visualization
diff --git a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
index 54354511..f4258463 100644
--- a/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
+++ b/opennav_coverage_demo/opennav_coverage_demo/demo_coverage.py
@@ -58,7 +58,7 @@ def toPolygon(self, field):
             poly.points.append(pt)
         return poly
 
-    def navigateCoverage(self, field):
+    def navigateCoverage(self, field, obstacle):
         """Send a `NavToPose` action request."""
         print("Waiting for 'NavigateCompleteCoverage' action server")
         while not self.coverage_client.wait_for_server(timeout_sec=1.0):
@@ -68,6 +68,10 @@ def navigateCoverage(self, field):
         goal_msg.frame_id = 'map'
         goal_msg.polygons.append(self.toPolygon(field))
 
+        if len(obstacle)>0:
+            for obs in obstacle:
+                goal_msg.polygons.append(self.toPolygon(obs))
+
         print('Navigating to with field of size: ' + str(len(field)) + '...')
         send_goal_future = self.coverage_client.send_goal_async(goal_msg,
                                                                 self._feedbackCallback)
@@ -146,8 +150,16 @@ def main():
     navigator.startup()
 
     # Some example field
-    field = [[5.0, 5.0], [5.0, 15.0], [15.0, 15.0], [10.0, 5.0], [5.0, 5.0]]
-    navigator.navigateCoverage(field)
+    # field = [[5.0, 5.0], [5.0, 15.0], [15.0, 15.0], [10.0, 5.0], [5.0, 5.0]]
+    # obstacle = [
+    #     [[10.0, 10.0], [10.0, 13.0], [13.0, 13.0], [13.0, 10.0], [10.0, 10.0]], 
+    #     [[8.0,8.0],[8.0,9.0],[9.0,9.0],[9.0,9.0], [8.0,8.0]]
+    #     ]
+    field = [[2.9, 20.5], [2.3, 18.8], [2.2, 17.4], [1.9, 15.6], [1.4, 13.7], [1.0, 12.1], [0.0, 10.8], [0.5, 10.0], [1.5, 9.8], [2.5, 9.6], [3.5, 9.2], [4.1, 8.1], [4.0, 7.0], [3.9, 6.0], [4.3, 5.8], [5.2, 6.0], [6.6, 5.8], [8.2, 5.7], [9.8, 5.5], [11.5, 5.3], [13.0, 5.0], [14.2, 5.2], [14.1, 6.1], [14.3, 7.5], [14.8, 8.6], [15.9, 9.0], [16.9, 8.5], [17.7, 8.1], [18.2, 7.1], [18.1, 6.0], [18.2, 4.8], [19.0, 4.0], [20.3, 3.5], [21.9, 3.3], [23.6, 3.3], [25.3, 3.0], [27.1, 2.7], [29.0, 2.6], [30.9, 2.5], [32.8, 2.2], [34.7, 2.0], [36.8, 1.7], [38.8, 1.4], [40.8, 1.3], [42.9, 1.2], [44.9, 0.9], [47.0, 0.7], [49.2, 0.5], [51.2, 0.3], [53.1, 0.1], [54.8, 0.5], [56.0, 1.5], [56.6, 2.9], [58.1, 3.9], [59.6, 3.6], [60.3, 2.6], [60.4, 1.3], [60.9, 0.8], [62.6, 0.7], [64.5, 0.3], [66.3, 0.0], [67.9, 0.0], [68.6, 0.5], [68.7, 2.1], [68.9, 4.1], [69.6, 6.0], [71.1, 6.4], [71.8, 6.9], [71.7, 8.3], [71.5, 10.2], [71.9, 11.9], [72.1, 13.6], [71.7, 14.2], [70.5, 14.6], [69.8, 15.8], [69.5, 16.8], [68.7, 17.5], [67.2, 17.4], [65.2, 17.2], [63.9, 16.9], [63.2, 15.5], [62.7, 13.6], [61.4, 12.0], [59.6, 11.0], [57.6, 10.7], [55.6, 10.9], [53.6, 11.1], [51.7, 11.2], [49.7, 11.3], [47.7, 11.5], [45.8, 12.0], [44.7, 13.6], [44.7, 15.3], [44.7, 17.1], [43.8, 17.9], [42.0, 18.0], [40.1, 18.1], [37.8, 18.3], [35.7, 18.6], [34.0, 18.8], [33.4, 18.3], [33.2, 16.6], [32.6, 14.8], [30.8, 13.6], [28.7, 13.3], [26.7, 13.6], [24.7, 14.2], [22.7, 14.6], [20.8, 14.7], [18.8, 14.7], [17.2, 15.6], [15.9, 16.6], [15.1, 18.0], [14.7, 19.6], [14.6, 21.2], [14.3, 22.6], [13.5, 24.3], [13.4, 26.4], [13.3, 28.2], [12.3, 29.0], [10.4, 29.5], [8.3, 29.7], [6.1, 29.6], [4.2, 29.5], [2.8, 29.1], [2.6, 27.7], [2.5, 25.8], [2.4, 24.0], [2.2, 22.3], [2.9, 20.5]]
+
+    obstacle = [[[36.7, 6.2], [36.7, 6.2], [36.8, 6.2], [38.0, 5.9], [39.5, 5.7], [41.0, 5.5], [42.5, 5.3], [43.9, 5.1], [44.6, 5.3], [44.8, 6.3], [44.8, 7.7], [44.9, 9.1], [44.8, 10.4], [43.9, 11.1], [42.5, 11.5], [40.8, 12.0], [39.2, 12.2], [37.7, 12.2], [36.7, 11.8], [36.1, 10.5], [35.8, 9.2], [35.7, 7.6], [35.9, 6.4], [36.4, 6.2], [36.7, 6.2]]]
+
+    navigator.navigateCoverage(field, obstacle)
 
     i = 0
     while not navigator.isTaskComplete():
diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt
index bf313e90..1e24f006 100644
--- a/opennav_row_coverage/CMakeLists.txt
+++ b/opennav_row_coverage/CMakeLists.txt
@@ -76,16 +76,16 @@ install(TARGETS ${executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-install(DIRECTORY include/
-  DESTINATION include/
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  find_package(ament_cmake_gtest REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-  add_subdirectory(test)
-endif()
+# install(DIRECTORY include/
+#   DESTINATION include/
+# )
+
+# if(BUILD_TESTING)
+#   find_package(ament_lint_auto REQUIRED)
+#   find_package(ament_cmake_gtest REQUIRED)
+#   ament_lint_auto_find_test_dependencies()
+#   add_subdirectory(test)
+# endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})

From 8e8b5a07341564c4d0e55483276691d904536733 Mon Sep 17 00:00:00 2001
From: TravisHC <745526203@qq.com>
Date: Mon, 9 Sep 2024 18:10:23 +0800
Subject: [PATCH 2/2] fix: add dep for test

---
 opennav_coverage/CMakeLists.txt     | 29 ++++++++++++++++-------------
 opennav_row_coverage/CMakeLists.txt | 21 +++++++++++----------
 2 files changed, 27 insertions(+), 23 deletions(-)

diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt
index 58575dd3..3b9b3931 100644
--- a/opennav_coverage/CMakeLists.txt
+++ b/opennav_coverage/CMakeLists.txt
@@ -57,7 +57,10 @@ ament_target_dependencies(${library_name}
   ${dependencies}
 )
 
-target_link_libraries(${library_name} Fields2Cover)
+target_link_libraries(${library_name} 
+  Fields2Cover
+  gdal)
+
 
 add_executable(${executable_name}
   src/main.cpp
@@ -85,18 +88,18 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-# install(FILES test/test_field.xml
-#               test/cartesian_test_field.xml
-#               test/irregular_test_field.xml
-#   DESTINATION share/${PROJECT_NAME}/
-# )
-
-# if(BUILD_TESTING)
-#   find_package(ament_lint_auto REQUIRED)
-#   find_package(ament_cmake_gtest REQUIRED)
-#   ament_lint_auto_find_test_dependencies()
-#   add_subdirectory(test)
-# endif()
+install(FILES test/test_field.xml
+              test/cartesian_test_field.xml
+              test/irregular_test_field.xml
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})
diff --git a/opennav_row_coverage/CMakeLists.txt b/opennav_row_coverage/CMakeLists.txt
index 1e24f006..bc99d044 100644
--- a/opennav_row_coverage/CMakeLists.txt
+++ b/opennav_row_coverage/CMakeLists.txt
@@ -52,7 +52,8 @@ ament_target_dependencies(${library_name}
 #and grab the vendored TINYXML2_LIBRARY too
 target_link_libraries(${library_name}
     Fields2Cover
-    tinyxml2::tinyxml2)
+    tinyxml2::tinyxml2
+    gdal)
 
 add_executable(${executable_name}
   src/main.cpp
@@ -76,16 +77,16 @@ install(TARGETS ${executable_name}
   RUNTIME DESTINATION lib/${PROJECT_NAME}
 )
 
-# install(DIRECTORY include/
-#   DESTINATION include/
-# )
+install(DIRECTORY include/
+  DESTINATION include/
+)
 
-# if(BUILD_TESTING)
-#   find_package(ament_lint_auto REQUIRED)
-#   find_package(ament_cmake_gtest REQUIRED)
-#   ament_lint_auto_find_test_dependencies()
-#   add_subdirectory(test)
-# endif()
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name})