Adding UTM conversion; tests complete show all quad works fine now after some finagling

SteveMacenski · SteveMacenski · commit f20044dbc489 · 2023-10-25T14:44:45.000-07:00
diff --git a/nav2_complete_coverage_msgs/action/ComputeCoveragePath.action b/nav2_complete_coverage_msgs/action/ComputeCoveragePath.action
@@ -8,6 +8,8 @@ bool generate_path True
 # The field specification to use.
 # If providing gml file with multiple fields, specify which with gml_field_id
 # If using polygons, bounding polygon must be first, followed by inner cutouts
+# Both must specify if the data is cartesian or GPS coordinates
+bool frame_cartesian True
 bool use_gml_file False
 string gml_field
 uint16 gml_field_id 0
diff --git a/nav2_coverage/README.md b/nav2_coverage/README.md
@@ -64,19 +64,10 @@ Walk through
     - RQT
 
 Future
-  - Transform GPS to UTM zone properly + back to GPS on return (https://github.com/Fields2Cover/Fields2Cover/pull/95/files)
   - Use coverage exceptions
-  - transform back into proper frame
 
   - BT nodes to interact with in in the Nav2 autonomy framework
   - A couple of utilities for the BT nodes to iterate through the swath-turn combos
   - Turn tester into Python API for it
   - A Navigator API plugin for coverage specific tasks
   - A BT XML to pair with the navigator + BT nodes to initial system demos
-
-Q
-  - Demo just have it follow the nav path on a coverage pattern, or something else more complex (or leave to you to determine?)
-    --> just normal path it up
-  - Other F2C add on features you care about? --> designed to expand or contribute back to F2C
-    - headland pass! 
-    - inner boundary not to go through (...) can wait, firefly might do.
diff --git a/nav2_coverage/include/nav2_coverage/utils.hpp b/nav2_coverage/include/nav2_coverage/utils.hpp
@@ -20,6 +20,7 @@
 #include <algorithm>
 #include <memory>
 
+#include "fields2cover.h" // NOLINT
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_coverage/types.hpp"
 #include "nav2_util/node_utils.hpp"
@@ -78,16 +79,25 @@ inline geometry_msgs::msg::PoseStamped toMsg(const PathState & state)
 /**
  * @brief Converts swaths to coverage path message for action client
  * @param swaths Swaths to convert. May be ordered or unordered.
+ * @param Field Field to use for conversion from UTM if necessary
+ * @param header header
+ * @param bool if the origional CRS is cartesian or not requiring conversion
  * @return PathComponents Info for action server to utilize
  */
 inline nav2_complete_coverage_msgs::msg::PathComponents toCoveragePathMsg(
-  const Swaths & swaths, const bool ordered, const std_msgs::msg::Header & header)
+  const Swaths & raw_swaths, const F2CField & field,
+  const bool ordered, const std_msgs::msg::Header & header, const bool is_cartesian)
 {
   nav2_complete_coverage_msgs::msg::PathComponents msg;
   msg.contains_turns = false;
   msg.swaths_ordered = ordered;
   msg.header = header;
-  msg.swaths.resize(swaths.size());
+  msg.swaths.resize(raw_swaths.size());
+
+  Swaths swaths = raw_swaths;
+  if (!is_cartesian) {
+    swaths = f2c::Transform::transformToPrevCRS(raw_swaths, field);
+  }
 
   for (unsigned int i = 0; i != swaths.size(); i++) {
     msg.swaths[i].start = toMsg(swaths[i].startPoint());
@@ -100,24 +110,33 @@ inline nav2_complete_coverage_msgs::msg::PathComponents toCoveragePathMsg(
 /**
  * @brief Converts full path to coverage path message for action client
  * @param path Full path to convert
+ * @param Field Field to use for conversion from UTM if necessary
+ * @param header header
+ * @param bool if the origional CRS is cartesian or not requiring conversion
  * @return PathComponents Info for action server to utilize
  */
 inline nav2_complete_coverage_msgs::msg::PathComponents toCoveragePathMsg(
-  const Path & path, const std_msgs::msg::Header & header)
+  const Path & raw_path, const F2CField & field,
+  const std_msgs::msg::Header & header, const bool is_cartesian)
 {
   using f2c::types::PathSectionType;
   nav2_complete_coverage_msgs::msg::PathComponents msg;
   msg.contains_turns = true;
   msg.swaths_ordered = true;
   msg.header = header;
 
-  if (path.states.size() == 0) {
+  if (raw_path.states.size() == 0) {
     return msg;
   }
 
   Point curr_swath_start(0.0, 0.0);
   nav_msgs::msg::Path * curr_turn = nullptr;
 
+  Path path = raw_path;
+  if (!is_cartesian) {
+    path = f2c::Transform::transformToPrevCRS(raw_path, field);
+  }
+
   PathSectionType curr_state = path.states[0].type;
   if (curr_state == PathSectionType::SWATH) {
     curr_swath_start = path.states[0].point;
@@ -176,19 +195,28 @@ inline nav2_complete_coverage_msgs::msg::PathComponents toCoveragePathMsg(
  * @brief Converts full path to nav_msgs/path message for action client, visualization
  * and use in direct-sending to a controller to replace the planner server
  * @param path Full path to convert
+ * @param Field Field to use for conversion from UTM if necessary
+ * @param header header
+ * @param bool if the origional CRS is cartesian or not requiring conversion
  * @return nav_msgs/Path Path
  */
 inline nav_msgs::msg::Path toNavPathMsg(
-  const Path & path, const std_msgs::msg::Header & header)
+  const Path & raw_path, const F2CField & field,
+  const std_msgs::msg::Header & header, const bool is_cartesian)
 {
   using f2c::types::PathSectionType;
   nav_msgs::msg::Path msg;
   msg.header = header;
 
-  if (path.states.size() == 0) {
+  if (raw_path.states.size() == 0) {
     return msg;
   }
 
+  Path path = raw_path;
+  if (!is_cartesian) {
+    path = f2c::Transform::transformToPrevCRS(raw_path, field);
+  }
+
   msg.poses.resize(path.states.size());
   for (unsigned int i = 0; i != path.states.size(); i++) {
     msg.poses[i] = toMsg(path.states[i]);
@@ -203,7 +231,7 @@ inline nav_msgs::msg::Path toNavPathMsg(
  * @param goal Goal to pase
  * @return Field field of goal polygons
  */
-inline Field getFieldFromGoal(
+inline F2CField getFieldFromGoal(
   typename std::shared_ptr<const typename ComputeCoveragePath::Goal> goal)
 {
   auto polygons = goal->polygons;
@@ -236,7 +264,7 @@ inline Field getFieldFromGoal(
     field.addRing(inner_polygon);
   }
 
-  return field;
+  return F2CField(Fields(field));
 }
 
 /**
diff --git a/nav2_coverage/src/coverage_server.cpp b/nav2_coverage/src/coverage_server.cpp
@@ -158,21 +158,26 @@ void CoverageServer::computeCoveragePath()
 
   try {
     // (0) Obtain field to use
+    bool cartesian_frame = goal->frame_cartesian;
     Field field;
+    F2CField master_field;
     std::string frame_id;
     if (goal->use_gml_file) {
       F2CFields parse_field;
       f2c::Parser::importGml(goal->gml_field, parse_field);
-      // TODO(SM): replace w/ UTM conversion (and back?) transformSwaths, transformPath
-      // Path rtn_path = f2c::Transform::transformPath(path, field, "EPSG:4258");
-      f2c::Transform::transform(parse_field[0], "EPSG:28992");
-      field = parse_field[goal->gml_field_id].field.getGeometry(0);
-      frame_id = parse_field[goal->gml_field_id].coord_sys;
+      master_field = parse_field[goal->gml_field_id];
+      frame_id = master_field.coord_sys;
     } else {
-      field = util::getFieldFromGoal(goal);
+      master_field = util::getFieldFromGoal(goal);
+      master_field.setCRS(goal->frame_id);
       frame_id = goal->frame_id;
     }
 
+    if (!cartesian_frame) {
+      f2c::Transform::transformToUTM(master_field);
+    }
+    field = master_field.field.getGeometry(0);
+
     RCLCPP_INFO(
       get_logger(),
       "Generating coverage path in %s frame for zone with %zu outer nodes and %zu inner polygons.",
@@ -195,19 +200,25 @@ void CoverageServer::computeCoveragePath()
       Swaths route = route_gen_->generateRoute(swaths, goal->route_mode);
 
       // (4) Optional: Generate connection turns between ordered swaths
+      // Converts UTM back to GPS, if necessary, for action returns
       if (goal->generate_path) {
         Path path = path_gen_->generatePath(route, goal->path_mode);
-        result->coverage_path = util::toCoveragePathMsg(path, header);
-        result->nav_path = util::toNavPathMsg(path, header);
+        result->coverage_path =
+          util::toCoveragePathMsg(path, master_field, header, cartesian_frame);
+        result->nav_path = util::toNavPathMsg(path, master_field, header, cartesian_frame);
       } else {
-        result->coverage_path = util::toCoveragePathMsg(route, true, header);
+        result->coverage_path =
+          util::toCoveragePathMsg(route, master_field, true, header, cartesian_frame);
       }
     } else {
-      result->coverage_path = util::toCoveragePathMsg(swaths, false, header);
+      result->coverage_path =
+        util::toCoveragePathMsg(swaths, master_field, false, header, cartesian_frame);
     }
 
     auto cycle_duration = this->now() - start_time;
     result->planning_time = cycle_duration;
+
+    // Visualization always uses cartesian coordinates for convenience
     visualizer_->visualize(field, field_no_headland, result, header);
     action_server_->succeeded_current(result);
   } catch (CoverageException & e) {
diff --git a/nav2_coverage/test/test_field.xml b/nav2_coverage/test/test_field.xml
@@ -5,6 +5,8 @@
                 <gml:outerBoundaryIs>
                     <gml:LinearRing>
                         <gml:coordinates>4.26199990317851,51.7859704975047 4.26203858931428,51.7860392024232 4.26234728296244,51.7867682098064 4.26283009086539,51.7878798462305 4.26321532471923,51.7887809885397 4.26344944239078,51.7893481817958 4.25601634525234,51.7906387243889 4.25603742148301,51.7905786368844 4.25662083047068,51.7889909449083 4.25732681137421,51.7870536505356 4.25749399420598,51.7866017400346 4.26195105582634,51.7858278333044 4.26199990317851,51.7859704975047</gml:coordinates>
+                        <!-- For Testing in Cartesian -->
+                        <!-- <gml:coordinates>0.0,0.0 2.78733,7.60181 25.3394,88.3711 60.5611,211.52 88.6878,311.358 105.814,374.206 -404.753,525.721 -403.403,519.014 -365.901,341.765 -320.543,125.493 -309.789,75.0412 -3.61568,-15.8186 0.0,0.0</gml:coordinates> -->
                     </gml:LinearRing>
                 </gml:outerBoundaryIs>
             </gml:Polygon>
diff --git a/nav2_coverage/test/test_server.cpp b/nav2_coverage/test/test_server.cpp
@@ -91,7 +91,8 @@ TEST(ServerTest, testServerTransactions)
     client_node, "compute_coverage_path");
 
   auto goal_msg = nav2_complete_coverage_msgs::action::ComputeCoveragePath::Goal();
-  goal_msg.use_gml_file = true;
+  goal_msg.use_gml_file = true;  // Use file
+  goal_msg.frame_cartesian = false;  // Using GPS coordinates
   goal_msg.gml_field =
     ament_index_cpp::get_package_share_directory("nav2_coverage") + "/test_field.xml";
 
diff --git a/nav2_coverage/test/test_utils.cpp b/nav2_coverage/test/test_utils.cpp
@@ -77,20 +77,21 @@ TEST(UtilsTests, TesttoCoveragePathMsg1)
   std_msgs::msg::Header header_in;
   header_in.frame_id = "test";
   Swaths swaths_in;
+  F2CField field;
 
   swaths_in.push_back(Swath());
   swaths_in.push_back(Swath());
   swaths_in.push_back(Swath());
 
-  auto msg = util::toCoveragePathMsg(swaths_in, ordered, header_in);
+  auto msg = util::toCoveragePathMsg(swaths_in, field, ordered, header_in, true);
   EXPECT_EQ(msg.header.frame_id, "test");
   EXPECT_EQ(msg.swaths_ordered, false);
   EXPECT_EQ(msg.swaths.size(), 3u);
   EXPECT_EQ(msg.contains_turns, false);
 
   ordered = true;
   swaths_in = Swaths();
-  msg = util::toCoveragePathMsg(swaths_in, ordered, header_in);
+  msg = util::toCoveragePathMsg(swaths_in, field, ordered, header_in, true);
   EXPECT_EQ(msg.swaths_ordered, true);
   EXPECT_EQ(msg.swaths.size(), 0u);
   EXPECT_EQ(msg.contains_turns, false);
@@ -102,8 +103,9 @@ TEST(UtilsTests, TesttoNavPathMsg)
   header_in.frame_id = "test";
   Path path_in;
   path_in.states.resize(10);
+  F2CField field;
 
-  auto msg = util::toNavPathMsg(path_in, header_in);
+  auto msg = util::toNavPathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.header.frame_id, "test");
   EXPECT_EQ(msg.poses.size(), 10u);
 }
@@ -113,15 +115,17 @@ TEST(UtilsTests, TesttoCoveragePathMsg2)
   std_msgs::msg::Header header_in;
   header_in.frame_id = "test";
   Path path_in;
-  auto msg = util::toCoveragePathMsg(path_in, header_in);
+  F2CField field;
+
+  auto msg = util::toCoveragePathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.swaths.size(), 0u);
   EXPECT_EQ(msg.turns.size(), 0u);
   EXPECT_EQ(msg.contains_turns, true);
   EXPECT_EQ(msg.swaths_ordered, true);
 
   // states are not valid (e.g. non-marked as turn or swath)
   path_in.states.resize(10);
-  EXPECT_THROW(util::toCoveragePathMsg(path_in, header_in), std::runtime_error);
+  EXPECT_THROW(util::toCoveragePathMsg(path_in, field, header_in, true), std::runtime_error);
 
   // Now lets make it valid
   using f2c::types::PathSectionType;
@@ -136,7 +140,7 @@ TEST(UtilsTests, TesttoCoveragePathMsg2)
   path_in.states[8].type = PathSectionType::SWATH;
   path_in.states[9].type = PathSectionType::SWATH;
 
-  msg = util::toCoveragePathMsg(path_in, header_in);
+  msg = util::toCoveragePathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.swaths.size(), 3u);
   EXPECT_EQ(msg.turns.size(), 2u);
   EXPECT_EQ(msg.turns[0].poses.size(), 2u);
@@ -152,7 +156,7 @@ TEST(UtilsTests, TesttoCoveragePathMsg2)
   path_in.states[8].type = PathSectionType::TURN;
   path_in.states[9].type = PathSectionType::TURN;
 
-  msg = util::toCoveragePathMsg(path_in, header_in);
+  msg = util::toCoveragePathMsg(path_in, field, header_in, true);
   EXPECT_EQ(msg.swaths.size(), 2u);
   EXPECT_EQ(msg.turns.size(), 3u);
   EXPECT_EQ(msg.turns[0].poses.size(), 2u);
@@ -174,7 +178,7 @@ TEST(UtilsTests, TestgetFieldFromGoal)
   // Should work now, with a trivial polygon of 3 nodes of (0, 0)
   goal->polygons[0].coordinates[2].axis1 = 0.0;
   auto field = util::getFieldFromGoal(goal);
-  EXPECT_EQ(field.getGeometry(0).size(), 3u);
+  EXPECT_EQ(field.field.getGeometry(0).getGeometry(0).size(), 3u);
 
   // Test with inner polygons, first invalid
   goal->polygons.resize(2);
@@ -185,8 +189,8 @@ TEST(UtilsTests, TestgetFieldFromGoal)
   // Now valid
   goal->polygons[0].coordinates[0].axis1 = 0.0;
   goal->polygons[1].coordinates[0].axis1 = 1.0;
-  field = util::getFieldFromGoal(goal);
-  EXPECT_EQ(field.getGeometry(1).size(), 3u);
+  auto field2 = util::getFieldFromGoal(goal);
+  EXPECT_EQ(field2.field.getGeometry(0).getGeometry(1).size(), 3u);
 }
 
 }  // namespace nav2_coverage
diff --git a/nav2_coverage/test/tester.py b/nav2_coverage/test/tester.py
@@ -16,6 +16,7 @@
 from enum import Enum
 
 from action_msgs.msg import GoalStatus
+from ament_index_python.packages import get_package_share_directory
 from lifecycle_msgs.srv import ChangeState, GetState
 from nav2_complete_coverage_msgs.action import ComputeCoveragePath
 from nav2_complete_coverage_msgs.msg import Coordinate  # Coordinates
@@ -124,19 +125,29 @@ def main():
     goal.generate_path = True
 
     goal.use_gml_file = True
-    goal.gml_field = '/home/steve/Documents/bonsai_ws/src/Fields2Cover/data/example1.xml'
+    goal.frame_cartesian = True
+    goal.gml_field = get_package_share_directory('nav2_coverage') + '/test_field.xml'
     goal.gml_field_id = 0
 
-    # Cartesian coordinates corresponding to a similar XML as example1.xml
+    # Cartesian coordinates corresponding to a similar XML as test_field.xml
     # test_coords = [[0.0, 0.0], [2.78733, 7.60181], [25.3394, 88.3711],
     #                [60.5611, 211.52], [88.6878, 311.358], [105.814, 374.206],
     #                [-404.753, 525.721], [-403.403, 519.014], [-365.901, 341.765],
     #                [-320.543, 125.493], [-309.789, 75.0412], [-3.61568, -15.8186], [0.0, 0.0]]
+
+    # Same, in "EPSG:4258" frame
+    # test_coords = [[4.26199990317851,51.7859704975047], [4.26203858931428,51.7860392024232],
+    #                [4.26234728296244,51.7867682098064], [4.26283009086539,51.7878798462305],
+    #                [4.26321532471923,51.7887809885397], [4.26344944239078,51.7893481817958],
+    #                [4.25601634525234,51.7906387243889], [4.25603742148301,51.7905786368844],
+    #                [4.25662083047068,51.7889909449083], [4.25732681137421,51.7870536505356],
+    #                [4.25749399420598,51.7866017400346], [4.26195105582634,51.7858278333044],
+    #                [4.26199990317851,51.7859704975047]]
     # coords = Coordinates()
     # for x in test_coords:
     #     coords.coordinates.append(toMsg(x))
     # goal.polygons.append(coords)
-    # goal.frame_id = "map"
+    # goal.frame_id = "map"  # "EPSG:4258"
 
     # goal.headland_mode.mode = "CONSTANT"
     # goal.headland_mode.width = 5.0
