Feature/mw/fields2cover2

.github/deps.repos

@@ -1,5 +1,9 @@
 repositories:
   Fields2Cover:
     type: git
-    url: https://github.com/Fields2Cover/Fields2Cover.git
+    url: https://github.com/aosmw/Fields2Cover.git
+    version: feature/mw/ros2_build
+  navigation2:
+    type: git
+    url: https://github.com/ros-planning/navigation2.git
     version: main

.github/workflows/test.yml

@@ -12,22 +12,63 @@ jobs:
       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-jammy-ros-rolling-ros-base-latest
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+          ros_distro: [rolling]
     steps:
+    - name: apt update
+      run: sudo apt update
+    - name: Install nodejs
+      run: sudo apt install nodejs -y
     - uses: actions/checkout@v2
     - name: Install Cyclone DDS
       run: sudo apt install ros-${{ matrix.ros_distro }}-rmw-cyclonedds-cpp -y
+    - name: Install geometry_msgs
+      run: sudo apt install ros-${{ matrix.ros_distro }}-geometry-msgs -y
+    - name: Install nav_msgs
+      run: sudo apt install ros-${{ matrix.ros_distro }}-nav-msgs -y
+    - name: Install rosidl-default-generators
+      run: sudo apt install ros-${{ matrix.ros_distro }}-rosidl-default-generators -y
+    - name: Install geographic_msgs
+      run: sudo apt install ros-${{ matrix.ros_distro }}-geographic-msgs -y
+    - name: Install bondcpp
+      run: sudo apt install ros-${{ matrix.ros_distro }}-bondcpp -y
+    - name: Install angles
+      run: sudo apt install ros-${{ matrix.ros_distro }}-angles -y
+    - name: Install test_msgs
+      run: sudo apt install ros-${{ matrix.ros_distro }}-test-msgs -y
+    - name: Install behaviortree_cpp
+      run: sudo apt install ros-${{ matrix.ros_distro }}-behaviortree-cpp -y
+    - name: Install diagnostic_updater
+      run: sudo apt install ros-${{ matrix.ros_distro }}-diagnostic-updater -y
+    - name: Install rviz_common
+      run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-common -y
+    - name: Install rviz_default_plugins
+      run: sudo apt install ros-${{ matrix.ros_distro }}-rviz-default-plugins -y
+    - name: Install ompl
+      run: sudo apt install ros-${{ matrix.ros_distro }}-ompl -y
+    - name: Install laser_geometry
+      run: sudo apt install ros-${{ matrix.ros_distro }}-laser-geometry -y
+    - name: Install cv_bridge
+      run: sudo apt install ros-${{ matrix.ros_distro }}-cv-bridge -y
+    - name: Install gazebo_ros_pkgs
+      run: sudo apt install ros-${{ matrix.ros_distro }}-gazebo-ros-pkgs -y
+    - name: Install libunwind-dev
+      run: sudo apt install libunwind-dev -y
+    - name: Install libceres-dev
+      run: sudo apt install libceres-dev -y
+    - name: Install robot_localization
+      run: sudo apt install ros-${{ matrix.ros_distro }}-robot-localization -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"
+        rosdep-check: true
     - uses: actions/upload-artifact@v1
       with:
         name: colcon-logs

.gitignore

@@ -0,0 +1,56 @@
+# 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/

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();

opennav_coverage/src/coverage_server.cpp

@@ -175,7 +175,7 @@ void CoverageServer::computeCoveragePath()
     std::string frame_id;
     if (goal->use_gml_file) {
       master_field = f2c::Parser::importFieldGml(goal->gml_field, true);
-      frame_id = master_field.coord_sys;
+      frame_id = master_field.getCRS();
     } else {
       master_field = util::getFieldFromGoal(goal);
       master_field.setCRS(goal->frame_id);
@@ -185,7 +185,7 @@ void CoverageServer::computeCoveragePath()
     if (!cartesian_frame_) {
       f2c::Transform::transformToUTM(master_field);
     }
-    field = master_field.field.getGeometry(0);
+    field = master_field.getField().getGeometry(0);
 
     RCLCPP_INFO(
       get_logger(),
@@ -269,10 +269,10 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
         path_gen_->setTurnPointDistance(parameter.as_double());
       } else if (name == "robot_width") {
         auto & robot = robot_params_->getRobot();
-        robot.robot_width = parameter.as_double();
+        robot.setWidth(parameter.as_double());
       } else if (name == "operation_width") {
         auto & robot = robot_params_->getRobot();
-        robot.op_width = parameter.as_double();
+        robot.setCovWidth(parameter.as_double());
       }
     } else if (type == ParameterType::PARAMETER_STRING) {
       if (name == "default_headland_type") {