Add TSP route ordering (Fields2Cover genRoute / OR-Tools) (Lyrical)

opennav_coverage/CMakeLists.txt

@@ -73,6 +73,7 @@ add_library(${library_name} SHARED
   src/headland_generator.cpp
   src/swath_generator.cpp
   src/route_generator.cpp
+  src/route_method.cpp
   src/path_generator.cpp
   src/visualizer.cpp
 )

opennav_coverage/include/opennav_coverage/path_generator.hpp

@@ -68,13 +68,13 @@ class PathGenerator
   }
 
   /**
-   * @brief Main method to generate path
-   * @param Swaths swaths to generate path from
-   * @param request Action request information
-   * @return Path complete path
+   * @brief Generate path from an F2CRoute (headland connections included).
+   * @param route Route produced by RouteGenerator::generateRoute
+   * @param settings PathMode for curve selection
+   * @return Path complete path including headland connections
    */
   Path generatePath(
-    const Swaths & swaths, const opennav_coverage_msgs::msg::PathMode & settings);
+    const F2CRoute & route, const opennav_coverage_msgs::msg::PathMode & settings);
 
   /**
    * @brief Sets the mode manually of the paths for dynamic parameters

opennav_coverage/include/opennav_coverage/route_generator.hpp

@@ -65,16 +65,38 @@ class RouteGenerator
         "default_custom_order was not set! "
         "If using Custom Route mode, the custom order must be set per-request!");
     }
+
+    nav2::declare_parameter_if_not_declared(
+      node, "default_tsp_redirect_swaths", rclcpp::ParameterValue(true));
+    default_tsp_redirect_swaths_ =
+      node->get_parameter("default_tsp_redirect_swaths").as_bool();
+
+    nav2::declare_parameter_if_not_declared(
+      node, "default_tsp_time_limit", rclcpp::ParameterValue(1));
+    default_tsp_time_limit_ =
+      node->get_parameter("default_tsp_time_limit").as_int();
+
+    nav2::declare_parameter_if_not_declared(
+      node, "default_tsp_search_for_optimum", rclcpp::ParameterValue(false));
+    default_tsp_search_for_optimum_ =
+      node->get_parameter("default_tsp_search_for_optimum").as_bool();
+
+    nav2::declare_parameter_if_not_declared(
+      node, "default_tsp_d_tol", rclcpp::ParameterValue(1e-4));
+    default_tsp_d_tol_ = node->get_parameter("default_tsp_d_tol").as_double();
   }
 
   /**
-   * @brief Main method to generate route
-   * @param Swaths swaths to generate route from
-   * @param request Action request information
-   * @return Swaths ordered swaths
+   * @brief Generate an ordered route for any mode (orderers or TSP).
+   * @param cells Travel cells whose borders route connections may follow
+   * @param swaths_by_cells Per-cell swaths from generateSwathsByCells
+   * @param settings Action request information
+   * @return F2CRoute (ordered swath groups plus any connections)
    */
-  Swaths generateRoute(
-    const Swaths & swaths, const opennav_coverage_msgs::msg::RouteMode & settings);
+  F2CRoute generateRoute(
+    const F2CCells & cells,
+    const F2CSwathsByCells & swaths_by_cells,
+    const opennav_coverage_msgs::msg::RouteMode & settings);
 
   /**
    * @brief Sets the mode manually of the Route for dynamic parameters
@@ -97,6 +119,11 @@ class RouteGenerator
     default_custom_order_ = std::vector<size_t>(order.begin(), order.end());
   }
 
+  void setTspRedirectSwaths(const bool v) {default_tsp_redirect_swaths_ = v;}
+  void setTspTimeLimit(const int v) {default_tsp_time_limit_ = v;}
+  void setTspSearchForOptimum(const bool v) {default_tsp_search_for_optimum_ = v;}
+  void setTspDTol(const double v) {default_tsp_d_tol_ = v;}
+
 protected:
   /**
    * @brief Creates generator pointer of a requested type
@@ -123,6 +150,10 @@ class RouteGenerator
   std::vector<size_t> default_custom_order_;
   size_t default_spiral_n_;
   RouteGeneratorPtr default_generator_{nullptr};
+  bool default_tsp_redirect_swaths_{true};
+  int default_tsp_time_limit_{1};
+  bool default_tsp_search_for_optimum_{false};
+  double default_tsp_d_tol_{1e-4};
   rclcpp::Logger logger_{rclcpp::get_logger("RouteGenerator")};
 };
 

opennav_coverage/include/opennav_coverage/route_method.hpp

@@ -0,0 +1,101 @@
+// Copyright (c) 2023 Open Navigation LLC
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OPENNAV_COVERAGE__ROUTE_METHOD_HPP_
+#define OPENNAV_COVERAGE__ROUTE_METHOD_HPP_
+
+#include <memory>
+#include <utility>
+
+#include "fields2cover.h" // NOLINT
+
+#include "rclcpp/rclcpp.hpp"
+#include "opennav_coverage_msgs/msg/route_mode.hpp"
+#include "opennav_coverage/types.hpp"
+
+namespace opennav_coverage
+{
+
+/**
+ * @class RouteMethod
+ * @brief Unifies F2C's two unrelated route planners (SingleCellSwathsOrderBase
+ *        and RoutePlannerBase) behind one polymorphic call returning F2CRoute,
+ *        so callers never branch on the route mode.
+ */
+class RouteMethod
+{
+public:
+  virtual ~RouteMethod() = default;
+
+  /**
+   * @brief Plan an ordered route over the swaths.
+   * @param cells Travel cells whose borders the route connections may follow
+   * @param swaths_by_cells Per-cell swaths to be covered
+   * @param settings Fully-resolved RouteMode (server has already applied defaults)
+   * @return Ordered route: swath groups plus any headland connections
+   */
+  virtual F2CRoute plan(
+    const F2CCells & cells,
+    const F2CSwathsByCells & swaths_by_cells,
+    const opennav_coverage_msgs::msg::RouteMode & settings) = 0;
+};
+
+/**
+ * @class SwathOrderMethod
+ * @brief Adapts the F2C swath-ordering modes (BOUSTROPHEDON, SNAKE, SPIRAL,
+ *        CUSTOM). Flattens the per-cell swaths, orders them with the wrapped
+ *        `SingleCellSwathsOrderBase`, and wraps the ordered swaths into a
+ *        single-group `F2CRoute` (no connections) so the output type matches TSP.
+ */
+class SwathOrderMethod : public RouteMethod
+{
+public:
+  SwathOrderMethod(
+    RouteType type, std::shared_ptr<f2c::rp::SingleCellSwathsOrderBase> orderer)
+  : type_(type), orderer_(std::move(orderer)) {}
+
+  F2CRoute plan(
+    const F2CCells & cells,
+    const F2CSwathsByCells & swaths_by_cells,
+    const opennav_coverage_msgs::msg::RouteMode & settings) override;
+
+private:
+  RouteType type_;
+  std::shared_ptr<f2c::rp::SingleCellSwathsOrderBase> orderer_;
+};
+
+/**
+ * @class TspRouteMethod
+ * @brief Adapts F2C's `RoutePlannerBase` (OR-Tools TSP). Solves each cell
+ *        separately and stitches the per-cell routes in sweep order, avoiding the
+ *        all-pairs path matrix that exhausts memory on decomposed multi-cell input.
+ */
+class TspRouteMethod : public RouteMethod
+{
+public:
+  explicit TspRouteMethod(const rclcpp::Logger & logger)
+  : logger_(logger) {}
+
+  F2CRoute plan(
+    const F2CCells & cells,
+    const F2CSwathsByCells & swaths_by_cells,
+    const opennav_coverage_msgs::msg::RouteMode & settings) override;
+
+private:
+  rclcpp::Logger logger_;
+};
+
+}  // namespace opennav_coverage
+
+#endif  // OPENNAV_COVERAGE__ROUTE_METHOD_HPP_

opennav_coverage/include/opennav_coverage/swath_generator.hpp

@@ -77,20 +77,12 @@ class SwathGenerator
   }
 
   /**
-   * @brief Main method to generate swaths in field for cell
-   * @param field Field to generate swaths from
-   * @param request Action request information
-   */
-  Swaths generateSwaths(
-    const Field & field, const opennav_coverage_msgs::msg::SwathMode & settings);
-
-  /**
-   * @brief Multi-cell overload: generate swaths across decomposed cells
+   * @brief Main method to generate swaths, per cell without flattening
    * @param cells Cells to generate swaths from
    * @param settings Action request information
-   * @return Flattened swaths across all cells
+   * @return Per-cell swaths (F2CSwathsByCells) — caller calls .flatten() if needed
    */
-  Swaths generateSwaths(
+  F2CSwathsByCells generateSwathsByCells(
     const F2CCells & cells, const opennav_coverage_msgs::msg::SwathMode & settings);
 
   /**

opennav_coverage/include/opennav_coverage/types.hpp

@@ -40,7 +40,10 @@ typedef F2CLineString LineString;
 typedef std::shared_ptr<f2c::hg::HeadlandGeneratorBase> HeadlandGeneratorPtr;
 typedef std::shared_ptr<f2c::obj::SGObjective> SwathObjectivePtr;
 typedef std::shared_ptr<f2c::pp::TurningBase> TurningBasePtr;
-typedef std::shared_ptr<f2c::rp::SingleCellSwathsOrderBase> RouteGeneratorPtr;
+
+// Polymorphic route abstraction unifying F2C's orderers + TSP planner; see route_method.hpp
+class RouteMethod;
+typedef std::shared_ptr<RouteMethod> RouteGeneratorPtr;
 
 typedef opennav_coverage_msgs::action::ComputeCoveragePath ComputeCoveragePath;
 
@@ -84,7 +87,8 @@ enum class RouteType
   BOUSTROPHEDON = 1,
   SNAKE = 2,
   SPIRAL = 3,
-  CUSTOM = 4
+  CUSTOM = 4,
+  TSP = 5
 };
 
 /**

opennav_coverage/include/opennav_coverage/utils.hpp

@@ -110,6 +110,27 @@ inline opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
   return msg;
 }
 
+/**
+ * @brief Converts an ordered route to coverage path message (ordered swaths only)
+ * @param route Route whose swath groups 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 opennav_coverage_msgs::msg::PathComponents toCoveragePathMsg(
+  const F2CRoute & route, const F2CField & field,
+  const std_msgs::msg::Header & header, const bool is_cartesian)
+{
+  Swaths ordered;
+  for (const auto & group : route.getVectorSwaths()) {
+    for (const auto & s : group) {
+      ordered.emplace_back(s);
+    }
+  }
+  return toCoveragePathMsg(ordered, field, true, header, is_cartesian);
+}
+
 /**
  * @brief Converts full path to coverage path message for action client
  * @param path Full path to convert

opennav_coverage/src/coverage_server.cpp

@@ -196,35 +196,37 @@ void CoverageServer::computeCoveragePath()
     const bool do_decomp = goal->generate_decomp || default_generate_decomp_;
 
     Field field_no_headland = field;
-    Swaths swaths;
+    F2CCells cells;
     if (do_decomp) {
       F2CCells raw_cells;
       raw_cells.addGeometry(field);
       F2CCells decomposed = decomp_gen_->decompose(raw_cells, goal->decomp_mode);
 
       // Apply a separate headland to each sub-cell
-      F2CCells cells_no_headland = decomposed;
-      if (goal->generate_headland) {
-        cells_no_headland = headland_gen_->generateHeadlands(decomposed, goal->headland_mode);
-      }
-      swaths = swath_gen_->generateSwaths(cells_no_headland, goal->swath_mode);
+      cells = goal->generate_headland ?
+        headland_gen_->generateHeadlands(decomposed, goal->headland_mode) : decomposed;
     } else {
       // (1) Optional: Remove headland from polygon field
       if (goal->generate_headland) {
         field_no_headland = headland_gen_->generateHeadlands(field, goal->headland_mode);
       }
-
-      // (2) Generate swaths to cover polygon field, including internal voids
-      swaths = swath_gen_->generateSwaths(field_no_headland, goal->swath_mode);
+      cells.addGeometry(field_no_headland);
     }
 
+    // (2) Generate swaths to cover polygon field, including internal voids
+    F2CSwathsByCells swaths_by_cells = swath_gen_->generateSwathsByCells(cells, goal->swath_mode);
+    Swaths swaths = swaths_by_cells.flatten();
+
     // (3) Optional: Generate an ordered route through the unordered swaths
     std_msgs::msg::Header header;
     header.stamp = now();
     header.frame_id = frame_id;
     Path path;
     if (goal->generate_route) {
-      Swaths route = route_gen_->generateRoute(swaths, goal->route_mode);
+      F2CRoute route = route_gen_->generateRoute(cells, swaths_by_cells, goal->route_mode);
+      if (route.isEmpty()) {
+        throw CoverageException("Route planner returned an empty route.");
+      }
 
       // (4) Optional: Generate connection turns between ordered swaths
       // Converts UTM back to GPS, if necessary, for action returns
@@ -238,8 +240,9 @@ void CoverageServer::computeCoveragePath()
         const double task_time = path.getTaskTime();
         result->task_time = std::isfinite(task_time) ? task_time : 0.0;
       } else {
+        // Ordered swaths only (no connecting turns)
         result->coverage_path =
-          util::toCoveragePathMsg(route, master_field, true, header, cartesian_frame_);
+          util::toCoveragePathMsg(route, master_field, header, cartesian_frame_);
       }
     } else {
       result->coverage_path =
@@ -287,6 +290,8 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
         swath_gen_->setStepAngle(parameter.as_double());
       } else if (name == "default_turn_point_distance") {
         path_gen_->setTurnPointDistance(parameter.as_double());
+      } else if (name == "default_tsp_d_tol") {
+        route_gen_->setTspDTol(parameter.as_double());
       } else if (name == "robot_width") {
         auto & robot = robot_params_->getRobot();
         robot.setWidth(parameter.as_double());
@@ -313,10 +318,16 @@ CoverageServer::dynamicParametersCallback(std::vector<rclcpp::Parameter> paramet
         swath_gen_->setOVerlap(parameter.as_bool());
       } else if (name == "coordinates_in_cartesian_frame") {
         cartesian_frame_ = parameter.as_bool();
+      } else if (name == "default_tsp_redirect_swaths") {
+        route_gen_->setTspRedirectSwaths(parameter.as_bool());
+      } else if (name == "default_tsp_search_for_optimum") {
+        route_gen_->setTspSearchForOptimum(parameter.as_bool());
       }
     } else if (type == ParameterType::PARAMETER_INTEGER) {
       if (name == "default_spiral_n") {
         route_gen_->setSpiralN(parameter.as_int());
+      } else if (name == "default_tsp_time_limit") {
+        route_gen_->setTspTimeLimit(parameter.as_int());
       }
     } else if (type == ParameterType::PARAMETER_INTEGER_ARRAY) {
       if (name == "default_custom_order") {

opennav_coverage/src/path_generator.cpp

@@ -21,7 +21,7 @@ namespace opennav_coverage
 {
 
 Path PathGenerator::generatePath(
-  const Swaths & swaths, const opennav_coverage_msgs::msg::PathMode & settings)
+  const F2CRoute & route, const opennav_coverage_msgs::msg::PathMode & settings)
 {
   PathType action_type = toType(settings.mode);
   PathContinuityType action_continuity_type = toContinuityType(settings.continuity_mode);
@@ -47,7 +47,7 @@ Path PathGenerator::generatePath(
     logger_,
     "Generating path with curve: %s", toString(action_type, action_continuity_type).c_str());
   curve->setDiscretization(turn_point_distance);
-  return generator_->planPath(robot_params_->getRobot(), swaths, *curve);
+  return generator_->planPath(robot_params_->getRobot(), route, *curve);
 }
 
 void PathGenerator::setPathMode(const std::string & new_mode)