Feature/mw/fields2cover2

.github/deps.repos

@@ -3,3 +3,11 @@ repositories:
     type: git
     url: https://github.com/Fields2Cover/Fields2Cover.git
     version: main
+  ortools_vendor:
+    type: git
+    url: https://github.com/Fields2Cover/ortools_vendor.git
+    version: main
+  navigation2:
+    type: git
+    url: https://github.com/ros-planning/navigation2/
+    version: main

.github/workflows/test.yml

@@ -16,7 +16,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-          ros_distro: [iron]
+          ros_distro: [rolling]
     steps:
     - uses: actions/checkout@v2
     - name: Install Cyclone DDS

.gitignore

@@ -0,0 +1 @@
+__pycache__/

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)
@@ -16,6 +17,7 @@ find_package(builtin_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(opennav_coverage_msgs REQUIRED)
 find_package(Fields2Cover REQUIRED)
+find_package(ortools_vendor REQUIRED)
 
 # potentially replace with nav2_common, nav2_package()
 set(CMAKE_CXX_STANDARD 17)
@@ -42,6 +44,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,45 +142,45 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
     path.moveTo(field.getRefPoint());
   }
 
-  PathSectionType curr_state = path.states[0].type;
+  PathSectionType curr_state = path.getState(0).type;
   if (curr_state == PathSectionType::SWATH) {
-    curr_swath_start = path.states[0].point;
+    curr_swath_start = path.getState(0).point;
   } else if (curr_state == 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++) {
+    if (curr_state == path.getState(i).type && path.getState(i).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_state == path.getState(i).type && path.getState(i).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_state != path.getState(i).type && path.getState(i).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_state != path.getState(i).type && path.getState(i).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_state = path.getState(i).type;
 
-    if (path.states[i].type != PathSectionType::SWATH &&
-      path.states[i].type != PathSectionType::TURN)
+    if (path.getState(i).type != PathSectionType::SWATH &&
+      path.getState(i).type != PathSectionType::TURN)
     {
       throw std::runtime_error("Unknown type of path state detected, cannot obtain path!");
     }
@@ -189,7 +189,7 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
   if (curr_state == 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 +215,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 +226,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 +243,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 +256,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/package.xml

@@ -19,6 +19,7 @@
   <depend>builtin_interfaces</depend>
   <depend>tf2_ros</depend>
   <depend>fields2cover</depend>
+  <depend>ortools_vendor</depend>
   <depend>opennav_coverage_msgs</depend>
 
   <test_depend>ament_lint_common</test_depend>

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") {

opennav_coverage/src/path_generator.cpp

@@ -26,17 +26,16 @@ Path PathGenerator::generatePath(
   PathType action_type = toType(settings.mode);
   PathContinuityType action_continuity_type = toContinuityType(settings.continuity_mode);
   std::shared_ptr<f2c::pp::TurningBase> curve{nullptr};
-  float turn_point_distance;
 
   // If not set by action, use default mode
   if (action_type == PathType::UNKNOWN || action_continuity_type == PathContinuityType::UNKNOWN) {
     action_type = default_type_;
     action_continuity_type = default_continuity_type_;
     curve = default_curve_;
-    turn_point_distance = default_turn_point_distance_;
+    curve->setDiscretization(default_turn_point_distance_);
   } else {
     curve = createCurve(action_type, action_continuity_type);
-    turn_point_distance = settings.turn_point_distance;
+    curve->setDiscretization(settings.turn_point_distance);
   }
 
   if (!curve) {
@@ -46,8 +45,7 @@ Path PathGenerator::generatePath(
   RCLCPP_DEBUG(
     logger_,
     "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str());
-  generator_->turn_point_dist = turn_point_distance;
-  return generator_->searchBestPath(robot_params_->getRobot(), swaths, *curve);
+  return generator_->planPath(robot_params_->getRobot(), swaths, *curve);
 }
 
 void PathGenerator::setPathMode(const std::string & new_mode)

opennav_coverage/src/swath_generator.cpp

@@ -51,7 +51,7 @@ Swaths SwathGenerator::generateSwaths(
       if (!objective) {
         throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE.");
       }
-      generator_->step_angle = step_angle;
+      generator_->setStepAngle(step_angle);
       return generator_->generateBestSwaths(*objective, robot_params_->getOperationWidth(), field);
     case SwathAngleType::SET_ANGLE:
       return generator_->generateSwaths(swath_angle, robot_params_->getOperationWidth(), field);

opennav_coverage/src/visualizer.cpp

@@ -46,6 +46,7 @@ void Visualizer::visualize(
     for (unsigned int i = 0; i != utm_path->poses.size(); i++) {
       utm_path->poses[i].pose.position.x += ref_pt.getX();
       utm_path->poses[i].pose.position.y += ref_pt.getY();
+      utm_path->poses[i].header.frame_id = GLOBAL_FRAME;  // Necessary for Foxglove
     }
     nav_plan_pub_->publish(std::move(utm_path));
   }

opennav_coverage/test/test_headland.cpp

@@ -80,13 +80,15 @@ TEST(HeadlandTests, TestheadlandGeneration)
 
   // Generate some toy field
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
 
   // Shouldn't throw, results in valid output
   opennav_coverage_msgs::msg::HeadlandMode settings;
-  auto field_out = generator.generateHeadlands(field.field.getGeometry(0), settings);
+  auto field_out = generator.generateHeadlands(field.getField().getGeometry(0), settings);
   settings.mode = "CONSTANT";
-  auto field_out2 = generator.generateHeadlands(field.field.getGeometry(0), settings);
+  auto field_out2 = generator.generateHeadlands(field.getField().getGeometry(0), settings);
 }
 
 }  // namespace opennav_coverage

opennav_coverage/test/test_path.cpp

@@ -114,9 +114,11 @@ TEST(PathTests, TestpathGeneration)
 
   // Generate some toy route
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
   opennav_coverage_msgs::msg::SwathMode sw_settings;
-  auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings);
+  auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings);
   opennav_coverage_msgs::msg::RouteMode rt_settings;
   auto route = route_gen.generateRoute(swaths, rt_settings);
 

opennav_coverage/test/test_robot.cpp

@@ -37,7 +37,7 @@ TEST(RobotTests, Testrobot)
 
   EXPECT_EQ(robot.getWidth(), 2.1);
   EXPECT_EQ(robot.getOperationWidth(), 2.5);
-  EXPECT_EQ(robot.getRobot().linear_curv_change, 2.0);
+  EXPECT_EQ(robot.getRobot().getMaxDiffCurv(), 2.0);
 }
 
 }  // namespace opennav_coverage

opennav_coverage/test/test_route.cpp

@@ -99,8 +99,10 @@ TEST(RouteTests, TestrouteGeneration)
 
   // Generate some toy field
   f2c::Random rand;
-  auto field = rand.generateRandField(5, 1e5);
-  auto swaths = swath_gen.generateSwaths(field.field.getGeometry(0), sw_settings);
+  double area = 1e5;
+  int sides = 5;
+  auto field = rand.generateRandField(area, sides);
+  auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings);
 
   // Shouldn't throw, results in valid output
   opennav_coverage_msgs::msg::RouteMode settings;