Feature/mw/fields2cover2

.github/deps.repos

@@ -1,5 +1,13 @@
 repositories:
-  Fields2Cover:
+  navigation2:
     type: git
-    url: https://github.com/Fields2Cover/Fields2Cover.git
+    url: https://github.com/ros-navigation/navigation2.git
     version: main
+  fields2cover:
+    type: git
+    url: https://github.com/aosmw/Fields2Cover.git
+    version: 9c3c352ba1dc71f3f48706c298b82cd4641e536e
+  ortools_vendor:
+    type: git
+    url: https://github.com/Fields2Cover/ortools_vendor.git
+    version: c28f53e50164645fa8cee0a7a307061ac6f83aa7

.github/workflows/test.yml

@@ -1,3 +1,4 @@
+---
 name: Test
 on:
   pull_request:
@@ -6,29 +7,46 @@ on:
       - main
 
 jobs:
-  build_and_test:
+  noble_rolling:
     runs-on: ubuntu-latest
     env:
       ROS_DISTRO: ${{ matrix.ros_distro }}
-      RMW_IMPLEMENTATION: rmw_cyclonedds_cpp
     container:
-      image: rostooling/setup-ros-docker:ubuntu-jammy-latest
+      image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+        ros_distro: [rolling]
     steps:
-    - uses: actions/checkout@v2
-    - name: Install Cyclone DDS
-      run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y
-    - name: Build and run tests
-      id: action-ros-ci
-      uses: ros-tooling/[email protected]
-      with:
-        import-token: ${{ secrets.GITHUB_TOKEN }}
-        target-ros2-distro: ${{ matrix.ros_distro }}
-        vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
-    - uses: actions/upload-artifact@v1
-      with:
-        name: colcon-logs
-        path: ros_ws/log
+      - name: Install node, libunwind-dev, libgoogle-glog-dev
+        run: |
+          sudo apt-get update
+          sudo apt-get -y install nodejs
+          sudo apt-get -y install libunwind-dev
+          sudo apt-get -y install libgoogle-glog-dev
+          sudo apt-get -y install libceres-dev
+      - name: checkout source
+        uses: actions/checkout@v4
+      - name: Build and run tests
+        id: action-ros-ci
+        uses: ros-tooling/[email protected]
+        with:
+          package-name: "opennav_coverage_demo"
+          target-ros2-distro: ${{ matrix.ros_distro }}
+          vcs-repo-file-url: "${{ github.workspace }}/.github/deps.repos"
+          colcon-defaults: |
+            {
+              "test": {
+                "packages-select": [
+                    "opennav_coverage",
+                    "opennav_coverage_bt",
+                    "opennav_row_coverage",
+                    "opennav_coverage_navigator",
+                    "opennav_coverage_demo"
+                ]
+              }
+            }
+      - uses: actions/upload-artifact@v1
+        with:
+          name: noble-colcon-logs
+          path: ros_ws/log

.gitignore

@@ -0,0 +1,59 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
+
+# Colcon output
+build
+log
+install
+
+# Visual Studio Code files
+.vscode
+
+# Eclipse project files
+.cproject
+.project
+.pydevproject
+
+# Python artifacts
+__pycache__/
+*.py[cod]
+.ipynb_checkpoints
+
+sphinx_doc/_build
+
+# CLion artifacts
+.idea
+cmake-build-debug/
+
+# doxygen docs
+doc/html/
+
+#Vim
+*.swp

README.md

@@ -1,6 +1,6 @@
 # Open Navigation's Nav2 Complete Coverage
 
-This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization. It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
+This package contains the Complete Coverage Task Server & auxiliary tools utilizing the [Fields2Cover](https://github.com/Fields2Cover/Fields2Cover) complete coverage planning system which includes a great deal of options in headland, swath, route, and final path planning. You can find more information about Fields2Cover (F2C) in its [ReadTheDocs Documentation](https://fields2cover.github.io/index.html). It can accept both GPS and Cartesian coordinates and publishes the field, headland, swaths, and route as separate topics in cartesian coordinates for debugging and visualization (topics have Transient Local durability for late-joining visualization tools). It can also compute coverage paths based on open-field polygons **or** based on annotated rows as might exist in a tree farm or other applications with both irregular and regular pre-established rows.
 
 This capability was created by [Open Navigation LLC](https://www.opennav.org/) in partnership with [Bonsai Robotics](https://www.bonsairobotics.ai/). Bonsai Robotics builds autonomous software for machines in adverse and GPS degraded conditions utilizing vision. Bonsai Robotics funded the development of this work for their own product and has graciously allowed Open Navigation to open-source it for the community to leverage in their own systems. Please thank Bonsai Robotics for their commendable donation to the ROS community! Bonsai is hiring [here](https://www.bonsairobotics.ai/careers).
 

opennav_coverage/CMakeLists.txt

@@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5)
 project(opennav_coverage)
 
 find_package(ament_cmake REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
@@ -42,6 +43,7 @@ set(dependencies
   builtin_interfaces
   tf2_ros
   opennav_coverage_msgs
+  ament_index_cpp
 )
 
 add_library(${library_name} SHARED

opennav_coverage/include/opennav_coverage/robot_params.hpp

@@ -44,19 +44,19 @@ class RobotParams
   {
     nav2_util::declare_parameter_if_not_declared(
       node, "robot_width", rclcpp::ParameterValue(2.1));
-    robot_.robot_width = node->get_parameter("robot_width").as_double();
+    robot_.setWidth(node->get_parameter("robot_width").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "operation_width", rclcpp::ParameterValue(2.5));
-    robot_.op_width = node->get_parameter("operation_width").as_double();
+    robot_.setCovWidth(node->get_parameter("operation_width").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "min_turning_radius", rclcpp::ParameterValue(0.4));
-    robot_.setMinRadius(node->get_parameter("min_turning_radius").as_double());
+    robot_.setMinTurningRadius(node->get_parameter("min_turning_radius").as_double());
 
     nav2_util::declare_parameter_if_not_declared(
       node, "linear_curv_change", rclcpp::ParameterValue(2.0));
-    robot_.linear_curv_change = node->get_parameter("linear_curv_change").as_double();
+    robot_.setMaxDiffCurv(node->get_parameter("linear_curv_change").as_double());
   }
 
   /**
@@ -65,16 +65,16 @@ class RobotParams
    */
   double getWidth()
   {
-    return robot_.robot_width;
+    return robot_.getWidth();
   }
 
   /**
-   * @brief Get the robot's operational width
+   * @brief Get the robot's operational/coverage width
    * @return robot's operational width in m
    */
   double getOperationWidth()
   {
-    return robot_.op_width;
+    return robot_.getCovWidth();
   }
 
   /**

opennav_coverage/include/opennav_coverage/utils.hpp

@@ -128,7 +128,7 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
   msg.swaths_ordered = true;
   msg.header = header;
 
-  if (raw_path.states.size() == 0) {
+  if (raw_path.size() == 0) {
     return msg;
   }
 
@@ -142,54 +142,55 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
     path.moveTo(field.getRefPoint());
   }
 
-  PathSectionType curr_state = path.states[0].type;
-  if (curr_state == PathSectionType::SWATH) {
-    curr_swath_start = path.states[0].point;
-  } else if (curr_state == PathSectionType::TURN) {
+  PathSectionType curr_section_type = path.getState(0).type;
+  if (curr_section_type == PathSectionType::SWATH) {
+    curr_swath_start = path.getState(0).point;
+  } else if (curr_section_type == PathSectionType::TURN) {
     msg.turns.push_back(nav_msgs::msg::Path());
     msg.turns.back().header = header;
     curr_turn = &msg.turns.back();
   }
 
-  for (unsigned int i = 0; i != path.states.size(); i++) {
-    if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::SWATH) {
+  for (unsigned int i = 0; i != path.size(); i++) {
+    auto idx_type = path.getState(i).type;
+    if (curr_section_type == idx_type && idx_type == PathSectionType::SWATH) {
       // Continuing swath so...
       // (1) no action required.
-    } else if (curr_state == path.states[i].type && path.states[i].type == PathSectionType::TURN) {
+    } else if (curr_section_type == idx_type && idx_type == PathSectionType::TURN) {
       // Continuing a turn so...
       // (1) keep populating
-      curr_turn->poses.push_back(toMsg(path.states[i]));
-    } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::TURN) {
+      curr_turn->poses.push_back(toMsg(path.getState(i)));
+    } else if (curr_section_type != idx_type && idx_type == PathSectionType::TURN) {
       // Transitioning from a swath to a turn so...
       // (1) Complete the existing swath
       opennav_coverage_msgs::msg::Swath swath;
       swath.start = toMsg(curr_swath_start);
-      swath.end = toMsg(path.states[i - 1].point);
+      swath.end = toMsg(path.getState(i - 1).point);
       msg.swaths.push_back(swath);
       // (2) Start a new turn path
       msg.turns.push_back(nav_msgs::msg::Path());
       msg.turns.back().header = header;
       curr_turn = &msg.turns.back();
-      curr_turn->poses.push_back(toMsg(path.states[i]));
-    } else if (curr_state != path.states[i].type && path.states[i].type == PathSectionType::SWATH) {
+      curr_turn->poses.push_back(toMsg(path.getState(i)));
+    } else if (curr_section_type != idx_type && idx_type == PathSectionType::SWATH) {
       // Transitioning from a turn to a swath so...
       // (1) Update new swath starting point
-      curr_swath_start = path.states[i].point;
+      curr_swath_start = path.getState(i).point;
     }
 
-    curr_state = path.states[i].type;
+    curr_section_type = idx_type;
 
-    if (path.states[i].type != PathSectionType::SWATH &&
-      path.states[i].type != PathSectionType::TURN)
+    if (idx_type != PathSectionType::SWATH &&
+      idx_type != PathSectionType::TURN)
     {
       throw std::runtime_error("Unknown type of path state detected, cannot obtain path!");
     }
   }
 
-  if (curr_state == PathSectionType::SWATH) {
+  if (curr_section_type == PathSectionType::SWATH) {
     opennav_coverage_msgs::msg::Swath swath;
     swath.start = toMsg(curr_swath_start);
-    swath.end = toMsg(path.states.back().point);
+    swath.end = toMsg(path.back().point);
     msg.swaths.push_back(swath);
   }
 
@@ -215,7 +216,7 @@ inline nav_msgs::msg::Path toNavPathMsg(
   nav_msgs::msg::Path msg;
   msg.header = header;
 
-  if (raw_path.states.size() == 0) {
+  if (raw_path.size() == 0) {
     return msg;
   }
 
@@ -226,15 +227,15 @@ inline nav_msgs::msg::Path toNavPathMsg(
     path.moveTo(field.getRefPoint());
   }
 
-  for (unsigned int i = 0; i != path.states.size(); i++) {
+  for (unsigned int i = 0; i != path.size(); i++) {
     // Swaths come in pairs of start-end sequentially
-    if (i > 0 && path.states[i].type == PathSectionType::SWATH &&
-      path.states[i - 1].type == PathSectionType::SWATH)
+    if (i > 0 && path.getState(i).type == PathSectionType::SWATH &&
+      path.getState(i - 1).type == PathSectionType::SWATH)
     {
-      const float & x0 = path.states[i - 1].point.getX();
-      const float & y0 = path.states[i - 1].point.getY();
-      const float & x1 = path.states[i].point.getX();
-      const float & y1 = path.states[i].point.getY();
+      const float & x0 = path.getState(i - 1).point.getX();
+      const float & y0 = path.getState(i - 1).point.getY();
+      const float & x1 = path.getState(i).point.getX();
+      const float & y1 = path.getState(i).point.getY();
 
       const float dist = hypotf(x1 - x0, y1 - y0);
       const float ux = (x1 - x0) / dist;
@@ -243,10 +244,10 @@ inline nav_msgs::msg::Path toNavPathMsg(
 
       geometry_msgs::msg::PoseStamped pose;
       pose.pose.orientation =
-        nav2_util::geometry_utils::orientationAroundZAxis(path.states[i].angle);
+        nav2_util::geometry_utils::orientationAroundZAxis(path.getState(i).angle);
       pose.pose.position.x = x0;
       pose.pose.position.y = y0;
-      pose.pose.position.z = path.states[i].point.getZ();
+      pose.pose.position.z = path.getState(i).point.getZ();
 
       while (curr_dist < dist) {
         pose.pose.position.x += pt_dist * ux;
@@ -256,7 +257,7 @@ inline nav_msgs::msg::Path toNavPathMsg(
       }
     } else {
       // Turns are already dense paths
-      msg.poses.push_back(toMsg(path.states[i]));
+      msg.poses.push_back(toMsg(path.getState(i)));
     }
   }
 

opennav_coverage/include/opennav_coverage/visualizer.hpp

@@ -46,16 +46,16 @@ class Visualizer
   {
     nav_plan_pub_ = rclcpp::create_publisher<nav_msgs::msg::Path>(
       node->get_node_topics_interface(),
-      "coverage_server/coverage_plan", rclcpp::QoS(1));
+      "coverage_server/coverage_plan", rclcpp::QoS(1).transient_local());
     headlands_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/field_boundary", rclcpp::QoS(1));
+      "coverage_server/field_boundary", rclcpp::QoS(1).transient_local());
     planning_field_pub_ = rclcpp::create_publisher<geometry_msgs::msg::PolygonStamped>(
       node->get_node_topics_interface(),
-      "coverage_server/planning_field", rclcpp::QoS(1));
+      "coverage_server/planning_field", rclcpp::QoS(1).transient_local());
     swaths_pub_ = rclcpp::create_publisher<visualization_msgs::msg::Marker>(
       node->get_node_topics_interface(),
-      "coverage_server/swaths", rclcpp::QoS(1));
+      "coverage_server/swaths", rclcpp::QoS(1).transient_local());
   }
 
   void deactivate();