From 7040fb36af1dfbe32c83ce9ac8cdff7806e700cf Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:18 +0300 Subject: [PATCH 1/7] Add TSP route mode via a RouteMethod abstraction Adds a TSP route orderer (Fields2Cover genRoute / OR-Tools) as a new TSP route mode alongside BOUSTROPHEDON / SNAKE / SPIRAL / CUSTOM. Introduces a RouteMethod polymorphic layer unifying F2C's swath orderers (SingleCellSwathsOrderBase) and the OR-Tools route planner (RoutePlannerBase) behind one F2CRoute-returning call, so callers no longer branch on the route mode. RouteGenerator resolves the mode to a RouteMethod and exposes the TSP knobs (tsp_redirect_swaths, tsp_time_limit, tsp_search_for_optimum, tsp_d_tol) as parameters and the RouteMode message. --- opennav_coverage/CMakeLists.txt | 1 + .../opennav_coverage/route_generator.hpp | 43 ++++++-- .../include/opennav_coverage/route_method.hpp | 101 ++++++++++++++++++ .../include/opennav_coverage/types.hpp | 8 +- opennav_coverage/src/route_generator.cpp | 56 ++++++---- opennav_coverage/src/route_method.cpp | 65 +++++++++++ opennav_coverage_msgs/msg/RouteMode.msg | 8 +- 7 files changed, 250 insertions(+), 32 deletions(-) create mode 100644 opennav_coverage/include/opennav_coverage/route_method.hpp create mode 100644 opennav_coverage/src/route_method.cpp diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index a35ce593..0de86b22 100644 --- a/opennav_coverage/CMakeLists.txt +++ b/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 ) diff --git a/opennav_coverage/include/opennav_coverage/route_generator.hpp b/opennav_coverage/include/opennav_coverage/route_generator.hpp index 361bbf9a..dd7265ed 100644 --- a/opennav_coverage/include/opennav_coverage/route_generator.hpp +++ b/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(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 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")}; }; diff --git a/opennav_coverage/include/opennav_coverage/route_method.hpp b/opennav_coverage/include/opennav_coverage/route_method.hpp new file mode 100644 index 00000000..5ed20951 --- /dev/null +++ b/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 +#include + +#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 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 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_ diff --git a/opennav_coverage/include/opennav_coverage/types.hpp b/opennav_coverage/include/opennav_coverage/types.hpp index 95ac6d48..7f7370aa 100644 --- a/opennav_coverage/include/opennav_coverage/types.hpp +++ b/opennav_coverage/include/opennav_coverage/types.hpp @@ -40,7 +40,10 @@ typedef F2CLineString LineString; typedef std::shared_ptr HeadlandGeneratorPtr; typedef std::shared_ptr SwathObjectivePtr; typedef std::shared_ptr TurningBasePtr; -typedef std::shared_ptr RouteGeneratorPtr; + +// Polymorphic route abstraction unifying F2C's orderers + TSP planner; see route_method.hpp +class RouteMethod; +typedef std::shared_ptr 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 }; /** diff --git a/opennav_coverage/src/route_generator.cpp b/opennav_coverage/src/route_generator.cpp index dfd7cc1d..e35499ae 100644 --- a/opennav_coverage/src/route_generator.cpp +++ b/opennav_coverage/src/route_generator.cpp @@ -16,41 +16,41 @@ #include #include "opennav_coverage/route_generator.hpp" +#include "opennav_coverage/route_method.hpp" namespace opennav_coverage { -Swaths RouteGenerator::generateRoute( - const Swaths & swaths, const opennav_coverage_msgs::msg::RouteMode & settings) +F2CRoute RouteGenerator::generateRoute( + const F2CCells & cells, const F2CSwathsByCells & swaths_by_cells, + const opennav_coverage_msgs::msg::RouteMode & settings) { RouteType action_type = toType(settings.mode); - std::shared_ptr generator{nullptr}; - size_t spiral_n; - std::vector custom_order; + + RouteGeneratorPtr method; + opennav_coverage_msgs::msg::RouteMode eff = settings; // If not set by action, use default mode if (action_type == RouteType::UNKNOWN) { action_type = default_type_; - generator = default_generator_; - spiral_n = default_spiral_n_; - custom_order = default_custom_order_; + method = default_generator_; + eff.spiral_n = default_spiral_n_; + eff.custom_order.assign(default_custom_order_.begin(), default_custom_order_.end()); + eff.tsp_redirect_swaths = default_tsp_redirect_swaths_; + eff.tsp_time_limit = default_tsp_time_limit_; + eff.tsp_search_for_optimum = default_tsp_search_for_optimum_; + eff.tsp_d_tol = default_tsp_d_tol_; } else { - generator = createGenerator(action_type); - spiral_n = settings.spiral_n; - custom_order = std::vector(settings.custom_order.begin(), settings.custom_order.end()); + method = createGenerator(action_type); } - if (!generator) { + if (!method) { throw CoverageException( - "No valid route mode set! Options: BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM."); - } else if (action_type == RouteType::SPIRAL) { - dynamic_cast(generator.get())->setSpiralSize(spiral_n); - } else if (action_type == RouteType::CUSTOM) { - dynamic_cast(generator.get())->setCustomOrder(custom_order); + "No valid route mode set! Options: BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM, TSP."); } - RCLCPP_DEBUG(logger_, "Generating route with generator: %s", toString(action_type).c_str()); - return generator->genSortedSwaths(swaths); + RCLCPP_DEBUG(logger_, "Generating route: %s", toString(action_type).c_str()); + return method->plan(cells, swaths_by_cells, eff); } void RouteGenerator::setMode(const std::string & new_mode) @@ -63,13 +63,19 @@ RouteGeneratorPtr RouteGenerator::createGenerator(const RouteType & type) { switch (type) { case RouteType::BOUSTROPHEDON: - return std::move(std::make_shared()); + return std::make_shared( + type, std::make_shared()); case RouteType::SNAKE: - return std::move(std::make_shared()); + return std::make_shared( + type, std::make_shared()); case RouteType::SPIRAL: - return std::move(std::make_shared()); + return std::make_shared( + type, std::make_shared()); case RouteType::CUSTOM: - return std::move(std::make_shared()); + return std::make_shared( + type, std::make_shared()); + case RouteType::TSP: + return std::make_shared(logger_); default: RCLCPP_WARN(logger_, "Unknown route type set!"); return RouteGeneratorPtr{nullptr}; @@ -87,6 +93,8 @@ std::string RouteGenerator::toString(const RouteType & type) return "Spiral"; case RouteType::CUSTOM: return "Custom"; + case RouteType::TSP: + return "TSP"; default: return "Unknown"; } @@ -104,6 +112,8 @@ RouteType RouteGenerator::toType(const std::string & str) return RouteType::SPIRAL; } else if (mode_str == "CUSTOM") { return RouteType::CUSTOM; + } else if (mode_str == "TSP") { + return RouteType::TSP; } else { return RouteType::UNKNOWN; } diff --git a/opennav_coverage/src/route_method.cpp b/opennav_coverage/src/route_method.cpp new file mode 100644 index 00000000..7caf6eee --- /dev/null +++ b/opennav_coverage/src/route_method.cpp @@ -0,0 +1,65 @@ +// 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. + +#include + +#include "opennav_coverage/route_method.hpp" + +namespace opennav_coverage +{ + +F2CRoute SwathOrderMethod::plan( + const F2CCells & /*cells*/, + const F2CSwathsByCells & swaths_by_cells, + const opennav_coverage_msgs::msg::RouteMode & settings) +{ + // The F2C orderers operate on a single flat swath list. + if (type_ == RouteType::SPIRAL) { + dynamic_cast(orderer_.get())->setSpiralSize(settings.spiral_n); + } else if (type_ == RouteType::CUSTOM) { + std::vector custom_order(settings.custom_order.begin(), settings.custom_order.end()); + dynamic_cast(orderer_.get())->setCustomOrder(custom_order); + } + + F2CSwaths ordered = orderer_->genSortedSwaths(swaths_by_cells.flatten()); + + // Wrap ordered swaths in a single-group route so every mode returns F2CRoute. + F2CRoute route; + route.addConnectedSwaths(F2CMultiPoint(), ordered); + return route; +} + +F2CRoute TspRouteMethod::plan( + const F2CCells & cells, + const F2CSwathsByCells & swaths_by_cells, + const opennav_coverage_msgs::msg::RouteMode & settings) +{ + const bool redirect_swaths = settings.tsp_redirect_swaths; + const long int time_limit = settings.tsp_time_limit; // NOLINT + const bool search_for_optimum = settings.tsp_search_for_optimum; + const double d_tol = settings.tsp_d_tol; + + RCLCPP_DEBUG( + logger_, + "Generating TSP route: redirect=%s time_limit=%ld optimum=%s d_tol=%f", + redirect_swaths ? "true" : "false", time_limit, + search_for_optimum ? "true" : "false", d_tol); + + f2c::rp::RoutePlannerBase rp; + return rp.genRoute( + cells, swaths_by_cells, + /*show_log=*/false, d_tol, redirect_swaths, time_limit, search_for_optimum); +} + +} // namespace opennav_coverage diff --git a/opennav_coverage_msgs/msg/RouteMode.msg b/opennav_coverage_msgs/msg/RouteMode.msg index e058b6b5..3beb2cfb 100644 --- a/opennav_coverage_msgs/msg/RouteMode.msg +++ b/opennav_coverage_msgs/msg/RouteMode.msg @@ -1,5 +1,11 @@ -string mode "UNKNOWN" # BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM +string mode "UNKNOWN" # BOUSTROPHEDON, SNAKE, SPIRAL, CUSTOM, TSP # Specific mode setting uint16 spiral_n 4 # If mode=SPIRAL, this is the number of swaths to spiral uint16[] custom_order # If mode=CUSTOM, this is the order of swaths to set. Must be specified. + +# TSP (OR-Tools) knobs — only used when mode=TSP +bool tsp_redirect_swaths true # Allow RoutePlannerBase to reverse swath direction for shorter turns +uint16 tsp_time_limit 1 # OR-Tools time limit in seconds (genRoute time_limit_seconds) +bool tsp_search_for_optimum false # Enable guided local search (slower but better quality) +float64 tsp_d_tol 0.0001 # Distance tolerance for OR-Tools (genRoute d_tol) From 26e9a5d87a32b8f7776bfca8921418b1b6fc58df Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:19 +0300 Subject: [PATCH 2/7] Route TSP per cell and stitch to avoid genRoute OOM A single multi-cell genRoute call makes RoutePlannerBase build the full all-pairs path matrix, which exhausts memory (bad_alloc) on decomposed input. The cells are disconnected anyway, so TspRouteMethod solves each cell alone and stitches the per-cell routes in sweep order with straight-line bridges. --- opennav_coverage/src/route_method.cpp | 41 ++++++++++++++++++++++++--- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/opennav_coverage/src/route_method.cpp b/opennav_coverage/src/route_method.cpp index 7caf6eee..9e6c0d76 100644 --- a/opennav_coverage/src/route_method.cpp +++ b/opennav_coverage/src/route_method.cpp @@ -56,10 +56,43 @@ F2CRoute TspRouteMethod::plan( redirect_swaths ? "true" : "false", time_limit, search_for_optimum ? "true" : "false", d_tol); - f2c::rp::RoutePlannerBase rp; - return rp.genRoute( - cells, swaths_by_cells, - /*show_log=*/false, d_tol, redirect_swaths, time_limit, search_for_optimum); + // Per-cell TSP instead of one multi-cell genRoute call: F2C v2.0.0's + // RoutePlannerBase builds the full all-pairs path matrix, which exhausts + // memory (bad_alloc) on decomposed input. The cells are disconnected anyway, + // so solve each one alone and stitch the routes with straight-line bridges. + F2CRoute merged; + for (size_t i = 0; i < cells.size(); ++i) { + if (i >= swaths_by_cells.size() || swaths_by_cells.at(i).size() == 0) { + continue; + } + F2CCells cell(cells.getGeometry(i)); + F2CSwathsByCells cell_swaths; + cell_swaths.emplace_back(swaths_by_cells.at(i)); + + f2c::rp::RoutePlannerBase rp; + F2CRoute cell_route = rp.genRoute( + cell, cell_swaths, + /*show_log=*/false, + d_tol, + redirect_swaths, + time_limit, + search_for_optimum); + if (cell_route.isEmpty()) { + continue; + } + + if (!merged.isEmpty()) { + merged.addConnection( + std::vector{merged.endPoint(), cell_route.startPoint()}); + } + const auto & vec_swaths = cell_route.getVectorSwaths(); + const auto & connections = cell_route.getConnections(); + for (size_t k = 0; k < vec_swaths.size(); ++k) { + merged.addConnectedSwaths( + k < connections.size() ? connections[k] : F2CMultiPoint(), vec_swaths[k]); + } + } + return merged; } } // namespace opennav_coverage From 35ab839d0a9858fe0e2819d47f3de212c706d0e2 Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:34 +0300 Subject: [PATCH 3/7] Plumb per-cell swaths and F2CRoute through swath/path generation SwathGenerator::generateSwathsByCells returns per-cell swaths (F2CSwathsByCells) without flattening, so the route planner can follow each cell's borders. PathGenerator::generatePath now takes the ordered F2CRoute (headland connections included) instead of raw swaths, and a toCoveragePathMsg overload converts an ordered route's swath groups for the no-turns result. --- .../opennav_coverage/path_generator.hpp | 10 +++--- .../opennav_coverage/swath_generator.hpp | 14 ++------ .../include/opennav_coverage/utils.hpp | 21 +++++++++++ opennav_coverage/src/path_generator.cpp | 4 +-- opennav_coverage/src/swath_generator.cpp | 35 +++---------------- 5 files changed, 35 insertions(+), 49 deletions(-) diff --git a/opennav_coverage/include/opennav_coverage/path_generator.hpp b/opennav_coverage/include/opennav_coverage/path_generator.hpp index b6d1febf..140ac715 100644 --- a/opennav_coverage/include/opennav_coverage/path_generator.hpp +++ b/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 diff --git a/opennav_coverage/include/opennav_coverage/swath_generator.hpp b/opennav_coverage/include/opennav_coverage/swath_generator.hpp index 591d7b1d..840b1120 100644 --- a/opennav_coverage/include/opennav_coverage/swath_generator.hpp +++ b/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); /** diff --git a/opennav_coverage/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index 79554634..4d6349b9 100644 --- a/opennav_coverage/include/opennav_coverage/utils.hpp +++ b/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 diff --git a/opennav_coverage/src/path_generator.cpp b/opennav_coverage/src/path_generator.cpp index 2528cb92..54ffaa11 100644 --- a/opennav_coverage/src/path_generator.cpp +++ b/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) diff --git a/opennav_coverage/src/swath_generator.cpp b/opennav_coverage/src/swath_generator.cpp index ae1b621c..b88bba9e 100644 --- a/opennav_coverage/src/swath_generator.cpp +++ b/opennav_coverage/src/swath_generator.cpp @@ -46,48 +46,21 @@ SwathGenerator::ResolvedSwathParams SwathGenerator::resolveSwathParams( return p; } -Swaths SwathGenerator::generateSwaths( - const Field & field, const opennav_coverage_msgs::msg::SwathMode & settings) -{ - ResolvedSwathParams p = resolveSwathParams(settings); - const double op_width = robot_params_->getOperationWidth(); - generator_->setAllowOverlap(default_allow_overlap_); - switch (p.angle_type) { - case SwathAngleType::BRUTE_FORCE: - if (!p.objective) { - throw CoverageException( - "No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE, NUMBER_MODIFIED."); - } - generator_->setStepAngle(p.step_angle); - return generator_->generateBestSwaths(*p.objective, op_width, field); - case SwathAngleType::SET_ANGLE: - return generator_->generateSwaths(p.swath_angle, op_width, field); - default: - throw CoverageException("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE."); - } -} - -Swaths SwathGenerator::generateSwaths( +F2CSwathsByCells SwathGenerator::generateSwathsByCells( const F2CCells & cells, const opennav_coverage_msgs::msg::SwathMode & settings) { - // Single cell -> use the existing Field path (no flatten needed) - if (cells.size() == 1) { - return generateSwaths(cells.getGeometry(0), settings); - } - ResolvedSwathParams p = resolveSwathParams(settings); const double op_width = robot_params_->getOperationWidth(); generator_->setAllowOverlap(default_allow_overlap_); switch (p.angle_type) { case SwathAngleType::BRUTE_FORCE: if (!p.objective) { - throw CoverageException( - "No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE, NUMBER_MODIFIED."); + throw CoverageException("No valid swath mode set! Options: LENGTH, NUMBER, COVERAGE."); } generator_->setStepAngle(p.step_angle); - return generator_->generateBestSwaths(*p.objective, op_width, cells).flatten(); + return generator_->generateBestSwaths(*p.objective, op_width, cells); case SwathAngleType::SET_ANGLE: - return generator_->generateSwaths(p.swath_angle, op_width, cells).flatten(); + return generator_->generateSwaths(p.swath_angle, op_width, cells); default: throw CoverageException("No valid swath angle mode set! Options: BRUTE_FORCE, SET_ANGLE."); } From 4eb5ce9645fa671c19c42c9bab6b81619698a71d Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:34 +0300 Subject: [PATCH 4/7] Wire TSP route ordering into the coverage servers coverage_server and row_coverage_server generate per-cell swaths, plan the ordered route (TSP or orderer) over them via RouteGenerator, and plan the connecting path over the resulting F2CRoute. An empty route is rejected, and the new TSP knobs are handled in the dynamic parameter callback. --- opennav_coverage/src/coverage_server.cpp | 33 ++++++++++++------- .../src/row_coverage_server.cpp | 13 ++++++-- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/opennav_coverage/src/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 591b55ae..994f69a7 100644 --- a/opennav_coverage/src/coverage_server.cpp +++ b/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 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 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") { diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index 15798898..2679a32c 100644 --- a/opennav_row_coverage/src/row_coverage_server.cpp +++ b/opennav_row_coverage/src/row_coverage_server.cpp @@ -198,7 +198,15 @@ void RowCoverageServer::computeCoveragePath() header.frame_id = frame_id; Path path; if (goal->generate_route) { - Swaths route = route_gen_->generateRoute(swaths, goal->route_mode); + // Single-cell group; the field polygon provides travel borders for TSP + F2CCells cells; + cells.addGeometry(field); + F2CSwathsByCells swaths_by_cells; + swaths_by_cells.emplace_back(swaths); + F2CRoute route = route_gen_->generateRoute(cells, swaths_by_cells, goal->route_mode); + if (route.isEmpty()) { + throw opennav_coverage::CoverageException("Route planner returned an empty route."); + } // (3) Optional: Generate connection turns between ordered swaths // Converts UTM back to GPS, if necessary, for action returns @@ -213,9 +221,10 @@ void RowCoverageServer::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 = opennav_coverage::util::toCoveragePathMsg( - route, master_field, true, header, cartesian_frame_); + route, master_field, header, cartesian_frame_); } } else { result->coverage_path = From 5151ac17a75a19b2d794f23b00c1f959bd592700 Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:34 +0300 Subject: [PATCH 5/7] Add TSP BT node ports for route mode control Exposes the TSP route knobs (tsp_redirect_swaths, tsp_time_limit, tsp_search_for_optimum) as input ports on the ComputeCompleteCoveragePath behavior tree node, forwarding them into the RouteMode goal. --- .../compute_complete_coverage_path.hpp | 6 ++++++ .../src/compute_complete_coverage_path.cpp | 13 +++++++++++++ 2 files changed, 19 insertions(+) diff --git a/opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp b/opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp index 73280f8d..e63b4d30 100644 --- a/opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp +++ b/opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp @@ -96,6 +96,12 @@ class ComputeCoveragePathAction BT::InputPort("generate_headland", true, "Whether to generate headland"), BT::InputPort("generate_route", true, "Whether to ordered route of swaths"), BT::InputPort("generate_path", true, "Whether to generate connected path of routes"), + BT::InputPort( + "route_mode_type", "UNKNOWN", "BOUSTROPHEDON/SNAKE/SPIRAL/CUSTOM/TSP"), + BT::InputPort("tsp_redirect_swaths", true, "TSP: allow swath direction reversal"), + BT::InputPort("tsp_time_limit", 1, "TSP: OR-Tools time limit in seconds"), + BT::InputPort("tsp_search_for_optimum", false, "TSP: guided local search"), + BT::InputPort("tsp_d_tol", 0.0001, "TSP: distance tolerance for OR-Tools"), BT::InputPort("file_field", "Filepath to field GML file"), BT::InputPort("file_field_id", 0, "Ordered ID of which field to use in GML File"), diff --git a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp index 82502db4..ad8613a9 100644 --- a/opennav_coverage_bt/src/compute_complete_coverage_path.cpp +++ b/opennav_coverage_bt/src/compute_complete_coverage_path.cpp @@ -40,6 +40,19 @@ void ComputeCoveragePathAction::on_tick() getInput("generate_route", goal_.generate_route); getInput("generate_path", goal_.generate_path); + // Route mode (pattern-order or TSP) + std::string route_mode_type; + getInput("route_mode_type", route_mode_type); + goal_.route_mode.mode = route_mode_type; + + // TSP knobs + getInput("tsp_redirect_swaths", goal_.route_mode.tsp_redirect_swaths); + int tsp_time_limit; + getInput("tsp_time_limit", tsp_time_limit); + goal_.route_mode.tsp_time_limit = static_cast(tsp_time_limit); + getInput("tsp_search_for_optimum", goal_.route_mode.tsp_search_for_optimum); + getInput("tsp_d_tol", goal_.route_mode.tsp_d_tol); + // Get the field to get coverage for std::string gml_filename; if (getInput("file_field", gml_filename)) { From 6ad6c5c4fb47557ec20cb37e273c957fef6fb0a6 Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 01:20:34 +0300 Subject: [PATCH 6/7] Add and update tests for TSP route ordering Covers TSP route generation, route-type resolution, and the TSP knobs across test_route / test_path / test_swath / test_server and the coverage BT tests. Bumps the test_server gtest timeout to 300s for the OR-Tools solve and raises the cancel BT service wait to 3000ms. --- opennav_coverage/test/CMakeLists.txt | 3 +- opennav_coverage/test/test_path.cpp | 37 +++++- opennav_coverage/test/test_route.cpp | 114 +++++++++++++++++- opennav_coverage/test/test_server.cpp | 107 +++++++++++++++- opennav_coverage/test/test_swath.cpp | 42 +++++-- .../test/test_cancel_complete_coverage.cpp | 2 +- .../test/test_compute_coverage_path.cpp | 34 ++++++ 7 files changed, 319 insertions(+), 20 deletions(-) diff --git a/opennav_coverage/test/CMakeLists.txt b/opennav_coverage/test/CMakeLists.txt index 53a19970..358846d2 100644 --- a/opennav_coverage/test/CMakeLists.txt +++ b/opennav_coverage/test/CMakeLists.txt @@ -62,9 +62,10 @@ target_link_libraries(test_path ${library_name} ) -# Test server + ament_add_gtest(test_server test_server.cpp + TIMEOUT 300 ) target_link_libraries(test_server ${library_name} diff --git a/opennav_coverage/test/test_path.cpp b/opennav_coverage/test/test_path.cpp index 4e01189b..7e01a07d 100644 --- a/opennav_coverage/test/test_path.cpp +++ b/opennav_coverage/test/test_path.cpp @@ -116,16 +116,49 @@ TEST(PathTests, TestpathGeneration) f2c::Random rand; auto field = rand.generateRandField(1e5, 5); opennav_coverage_msgs::msg::SwathMode sw_settings; - auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); opennav_coverage_msgs::msg::RouteMode rt_settings; - auto route = route_gen.generateRoute(swaths, rt_settings); + auto route = route_gen.generateRoute(cells, sbc, rt_settings); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::PathMode settings; auto path1 = generator.generatePath(route, settings); + EXPECT_GT(path1.size(), 0u); + EXPECT_TRUE(std::isfinite(path1.getTaskTime())); settings.mode = "REEDS_SHEPP"; settings.continuity_mode = "CONTINUOUS"; auto path2 = generator.generatePath(route, settings); + EXPECT_GT(path2.size(), 0u); +} + +TEST(PathTests, TestpathGenerationFromF2CRoute) +{ + // B1-T6: generatePath(F2CRoute) overload returns non-empty path with finite task time + auto node = std::make_shared("test_node"); + RobotParams robot_params(node); + SwathGenerator swath_gen(node, &robot_params); + RouteGenerator route_gen(node); + PathShim generator(node, &robot_params); + + f2c::Random rand; + auto field = rand.generateRandField(1e5, 5); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + + opennav_coverage_msgs::msg::SwathMode sw_settings; + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); + + opennav_coverage_msgs::msg::RouteMode rt_settings; + rt_settings.mode = "TSP"; + F2CRoute tsp_route = route_gen.generateRoute(cells, sbc, rt_settings); + ASSERT_FALSE(tsp_route.isEmpty()); + + opennav_coverage_msgs::msg::PathMode path_settings; + auto path = generator.generatePath(tsp_route, path_settings); + EXPECT_GT(path.size(), 0u); + EXPECT_TRUE(std::isfinite(path.getTaskTime())); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_route.cpp b/opennav_coverage/test/test_route.cpp index 57374f23..a582817a 100644 --- a/opennav_coverage/test/test_route.cpp +++ b/opennav_coverage/test/test_route.cpp @@ -77,12 +77,18 @@ TEST(RouteTests, TestrouteUtils) EXPECT_EQ(generator.toStringShim(RouteType::SNAKE), std::string("Snake")); EXPECT_EQ(generator.toStringShim(RouteType::SPIRAL), std::string("Spiral")); EXPECT_EQ(generator.toStringShim(RouteType::CUSTOM), std::string("Custom")); + // B1-T11: TSP toType/toString + EXPECT_EQ(generator.toTypeShim("TSP"), RouteType::TSP); + EXPECT_EQ(generator.toTypeShim("tsp"), RouteType::TSP); + EXPECT_EQ(generator.toStringShim(RouteType::TSP), std::string("TSP")); EXPECT_TRUE(generator.createGeneratorShim(RouteType::BOUSTROPHEDON)); EXPECT_TRUE(generator.createGeneratorShim(RouteType::SNAKE)); EXPECT_TRUE(generator.createGeneratorShim(RouteType::SPIRAL)); EXPECT_TRUE(generator.createGeneratorShim(RouteType::CUSTOM)); EXPECT_FALSE(generator.createGeneratorShim(RouteType::UNKNOWN)); + // TSP is now a valid RouteMethod (TspRouteMethod adapting RoutePlannerBase) + EXPECT_TRUE(generator.createGeneratorShim(RouteType::TSP)); generator.setMode("a mode"); generator.setSpiralN(10); @@ -100,19 +106,117 @@ TEST(RouteTests, TestrouteGeneration) // Generate some toy field f2c::Random rand; auto field = rand.generateRandField(1e5, 5); - auto swaths = swath_gen.generateSwaths(field.getField().getGeometry(0), sw_settings); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::RouteMode settings; - auto route1 = generator.generateRoute(swaths, settings); + auto route1 = generator.generateRoute(cells, sbc, settings); settings.mode = "BOUSTROPHEDON"; - auto route2 = generator.generateRoute(swaths, settings); + auto route2 = generator.generateRoute(cells, sbc, settings); settings.mode = "SPIRAL"; - auto route3 = generator.generateRoute(swaths, settings); + auto route3 = generator.generateRoute(cells, sbc, settings); // Throws since custom order is set to emptry set settings.mode = "CUSTOM"; - EXPECT_THROW(generator.generateRoute(swaths, settings), std::length_error); + EXPECT_THROW(generator.generateRoute(cells, sbc, settings), std::length_error); +} + +TEST(RouteTests, TestTSPGeneration) +{ + auto node = std::make_shared("test_node"); + RobotParams robot_params(node); + SwathGenerator swath_gen(node, &robot_params); + RouteShim generator(node); + + // Generate a two-cell field for TSP (B1-T1: non-empty route, B1-T2: coverage, B1-T3: connections) + f2c::Random rand; + auto field = rand.generateRandField(1e5, 5); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + + opennav_coverage_msgs::msg::SwathMode sw_settings; + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); + + opennav_coverage_msgs::msg::RouteMode settings; + settings.mode = "TSP"; + settings.tsp_redirect_swaths = true; + settings.tsp_time_limit = 1; + settings.tsp_search_for_optimum = false; + settings.tsp_d_tol = 1e-4; + + // B1-T1: route is non-empty + F2CRoute route = generator.generateRoute(cells, sbc, settings); + EXPECT_FALSE(route.isEmpty()); + EXPECT_GE(route.sizeVectorSwaths(), 1u); + EXPECT_GT(route.length(), 0.0); + + // B1-T2: swath count is preserved + size_t total_in = sbc.sizeTotal(); + size_t total_out = 0; + for (size_t i = 0; i < route.sizeVectorSwaths(); ++i) { + total_out += route.getVectorSwaths()[i].size(); + } + EXPECT_EQ(total_in, total_out); + + // B1-T3: connection count is consistent (>= 0, asLineString non-empty) + EXPECT_GE(route.sizeConnections(), 0u); + EXPECT_GT(route.asLineString().size(), 0u); +} + +TEST(RouteTests, TestTSPSingleCell) +{ + // B1-T4: single cell input should not crash + auto node = std::make_shared("test_node"); + RobotParams robot_params(node); + SwathGenerator swath_gen(node, &robot_params); + RouteShim generator(node); + + f2c::Random rand; + auto field = rand.generateRandField(1e5, 5); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + + opennav_coverage_msgs::msg::SwathMode sw_settings; + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); + + opennav_coverage_msgs::msg::RouteMode settings; + settings.mode = "TSP"; + + F2CRoute route = generator.generateRoute(cells, sbc, settings); + EXPECT_FALSE(route.isEmpty()); +} + +TEST(RouteTests, TestSwathOrderWrappedAsRoute) +{ + // Orderer modes return their sorted swaths wrapped in a single-group F2CRoute; + // check the wrap keeps one group and preserves the swath count. + auto node = std::make_shared("test_node"); + RobotParams robot_params(node); + SwathGenerator swath_gen(node, &robot_params); + RouteShim generator(node); + + f2c::Random rand; + auto field = rand.generateRandField(1e5, 5); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); + + opennav_coverage_msgs::msg::SwathMode sw_settings; + F2CSwathsByCells sbc = swath_gen.generateSwathsByCells(cells, sw_settings); + + opennav_coverage_msgs::msg::RouteMode settings; + settings.mode = "BOUSTROPHEDON"; + F2CRoute route = generator.generateRoute(cells, sbc, settings); + + EXPECT_FALSE(route.isEmpty()); + EXPECT_EQ(route.sizeVectorSwaths(), 1u); + + size_t total_out = 0; + for (size_t i = 0; i < route.sizeVectorSwaths(); ++i) { + total_out += route.getVectorSwaths()[i].size(); + } + EXPECT_EQ(sbc.sizeTotal(), total_out); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_server.cpp b/opennav_coverage/test/test_server.cpp index b545ed82..1fd2e420 100644 --- a/opennav_coverage/test/test_server.cpp +++ b/opennav_coverage/test/test_server.cpp @@ -214,6 +214,101 @@ TEST(ServerTest, testDecompPathNoHeadland) EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); } +TEST(ServerTest, testTSPRouteNoPath) +{ + // TSP with generate_path=false now succeeds (returns ordered swaths) instead of + // being rejected — the unified route path removed the old guard. + auto node = std::make_shared(); + rclcpp_lifecycle::State state; + node->configure(state); + node->activate(state); + auto node_thread = std::make_unique(node); + + auto client_node = std::make_shared("my_node_tsp_nopath"); + auto action_client = + rclcpp_action::create_client( + client_node, "compute_coverage_path"); + action_client->wait_for_action_server(); + + auto goal_msg = opennav_coverage_msgs::action::ComputeCoveragePath::Goal(); + goal_msg.use_gml_file = true; + goal_msg.generate_route = true; + goal_msg.generate_path = false; + goal_msg.route_mode.mode = "TSP"; + goal_msg.route_mode.tsp_time_limit = 1; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + const std::filesystem::path share_dir = + ament_index_cpp::get_package_share_directory("opennav_coverage"); +#pragma GCC diagnostic pop + goal_msg.gml_field = (share_dir / "test_field.xml").string(); + + auto future_goal_handle = action_client->async_send_goal(goal_msg); + EXPECT_EQ( + rclcpp::spin_until_future_complete(client_node, future_goal_handle), + rclcpp::FutureReturnCode::SUCCESS); + auto goal_handle = future_goal_handle.get(); + + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_FALSE(result.result->coverage_path.swaths.empty()); +} + +TEST(ServerTest, testTSPDecompHeadlandPath) +{ + // Exercises the TSP branch end-to-end with decomp+headland enabled together: + // trapezoidal decomposition yields ~11 disconnected (headland-shrunk) cells, + // which RouteGenerator solves as per-cell TSP stitched in sweep order. + auto node = std::make_shared(); + rclcpp_lifecycle::State state; + node->configure(state); + node->activate(state); + auto node_thread = std::make_unique(node); + + auto client_node = std::make_shared("my_node_tsp_decomp_hl"); + auto action_client = + rclcpp_action::create_client( + client_node, "compute_coverage_path"); + action_client->wait_for_action_server(); + + auto goal_msg = opennav_coverage_msgs::action::ComputeCoveragePath::Goal(); + goal_msg.use_gml_file = true; + goal_msg.generate_decomp = true; + goal_msg.decomp_mode.mode = "TRAPEZOIDAL"; + goal_msg.generate_headland = true; + goal_msg.generate_route = true; + goal_msg.generate_path = true; + goal_msg.route_mode.mode = "TSP"; + // Per-cell OR-Tools limit; keeps the 11-cell total bounded in CI. + goal_msg.route_mode.tsp_time_limit = 1; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + const std::filesystem::path share_dir = + ament_index_cpp::get_package_share_directory("opennav_coverage"); +#pragma GCC diagnostic pop + goal_msg.gml_field = (share_dir / "test_field.xml").string(); + + auto future_goal_handle = action_client->async_send_goal(goal_msg); + EXPECT_EQ( + rclcpp::spin_until_future_complete(client_node, future_goal_handle), + rclcpp::FutureReturnCode::SUCCESS); + auto goal_handle = future_goal_handle.get(); + + auto future_result = action_client->async_get_result(goal_handle); + EXPECT_EQ( + rclcpp::spin_until_future_complete(client_node, future_result), + rclcpp::FutureReturnCode::SUCCESS); + + auto result = future_result.get(); + EXPECT_EQ(result.code, rclcpp_action::ResultCode::SUCCEEDED); + EXPECT_FALSE(result.result->nav_path.poses.empty()); +} + TEST(ServerTest, testDynamicParams) { auto node = std::make_shared(); @@ -242,7 +337,12 @@ TEST(ServerTest, testDynamicParams) rclcpp::Parameter("default_allow_overlap", true), rclcpp::Parameter("default_spiral_n", 41), rclcpp::Parameter("coordinates_in_cartesian_frame", false), - rclcpp::Parameter("default_custom_order", std::vector{1, 2, 3})}); + rclcpp::Parameter("default_custom_order", std::vector{1, 2, 3}), + // B1-T13: TSP dynamic params + rclcpp::Parameter("default_tsp_redirect_swaths", false), + rclcpp::Parameter("default_tsp_time_limit", 5), + rclcpp::Parameter("default_tsp_search_for_optimum", true), + rclcpp::Parameter("default_tsp_d_tol", 1e-3)}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -253,6 +353,11 @@ TEST(ServerTest, testDynamicParams) EXPECT_EQ(node->get_parameter("default_allow_overlap").as_bool(), true); EXPECT_EQ(node->get_parameter("default_spiral_n").as_int(), 41); EXPECT_EQ(node->get_parameter("coordinates_in_cartesian_frame").as_bool(), false); + // B1-T13: verify TSP params are declared and callback-connected + EXPECT_EQ(node->get_parameter("default_tsp_redirect_swaths").as_bool(), false); + EXPECT_EQ(node->get_parameter("default_tsp_time_limit").as_int(), 5); + EXPECT_EQ(node->get_parameter("default_tsp_search_for_optimum").as_bool(), true); + EXPECT_NEAR(node->get_parameter("default_tsp_d_tol").as_double(), 1e-3, 1e-9); } } // namespace opennav_coverage diff --git a/opennav_coverage/test/test_swath.cpp b/opennav_coverage/test/test_swath.cpp index 5381e362..df3e1f2e 100644 --- a/opennav_coverage/test/test_swath.cpp +++ b/opennav_coverage/test/test_swath.cpp @@ -116,19 +116,21 @@ TEST(SwathTests, TestswathGeneration) // Generate some toy field f2c::Random rand; auto field = rand.generateRandField(1e5, 5); + F2CCells cells; + cells.addGeometry(field.getField().getGeometry(0)); // Shouldn't throw, results in valid output opennav_coverage_msgs::msg::SwathMode settings; - auto swaths1 = generator.generateSwaths(field.getField().getGeometry(0), settings); + auto swaths1 = generator.generateSwathsByCells(cells, settings); settings.mode = "BRUTE_FORCE"; settings.objective = "LENGTH"; - auto swaths2 = generator.generateSwaths(field.getField().getGeometry(0), settings); + auto swaths2 = generator.generateSwathsByCells(cells, settings); settings.mode = "SET_ANGLE"; settings.objective = "NUMBER"; - auto swaths3 = generator.generateSwaths(field.getField().getGeometry(0), settings); + auto swaths3 = generator.generateSwathsByCells(cells, settings); settings.mode = "BRUTE_FORCE"; settings.objective = "NUMBER_MODIFIED"; - auto swaths4 = generator.generateSwaths(field.getField().getGeometry(0), settings); + auto swaths4 = generator.generateSwathsByCells(cells, settings); } TEST(SwathTests, TestswathGenerationMultiCell) @@ -143,18 +145,38 @@ TEST(SwathTests, TestswathGenerationMultiCell) opennav_coverage_msgs::msg::SwathMode settings; - // Single-cell F2CCells routes through the Field path F2CCells one_cell; one_cell.addGeometry(cell); - auto swaths_one = generator.generateSwaths(one_cell, settings); - EXPECT_GT(swaths_one.size(), 0u); + auto swaths_one = generator.generateSwathsByCells(one_cell, settings); + EXPECT_GT(swaths_one.sizeTotal(), 0u); - // Multi-cell F2CCells flattens swaths across cells + // Multi-cell input yields swaths per cell F2CCells cells; cells.addGeometry(cell); cells.addGeometry(cell); - auto swaths_multi = generator.generateSwaths(cells, settings); - EXPECT_GT(swaths_multi.size(), swaths_one.size()); + auto swaths_multi = generator.generateSwathsByCells(cells, settings); + EXPECT_GT(swaths_multi.sizeTotal(), swaths_one.sizeTotal()); +} + +TEST(SwathTests, TestgenerateSwathsByCells) +{ + // Per-cell structure has one entry per cell; flatten() keeps every swath + auto node = std::make_shared("test_node"); + RobotParams robot(node); + auto generator = SwathShim(node, &robot); + + f2c::Random rand; + auto field = rand.generateRandField(1e5, 5); + Field cell = field.getField().getGeometry(0); + F2CCells cells; + cells.addGeometry(cell); + cells.addGeometry(cell); + + opennav_coverage_msgs::msg::SwathMode settings; + + F2CSwathsByCells sbc = generator.generateSwathsByCells(cells, settings); + EXPECT_EQ(sbc.size(), cells.size()); + EXPECT_EQ(sbc.flatten().size(), sbc.sizeTotal()); } } // namespace opennav_coverage diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp index 9a6491b8..e59937c6 100644 --- a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp +++ b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp @@ -68,7 +68,7 @@ class CancelCoverageActionTestFixture : public ::testing::Test std::chrono::milliseconds(10)); config_->blackboard->set( "wait_for_service_timeout", - std::chrono::milliseconds(1000)); + std::chrono::milliseconds(3000)); client_ = rclcpp_action::create_client( node_, "compute_coverage_path"); diff --git a/opennav_coverage_bt/test/test_compute_coverage_path.cpp b/opennav_coverage_bt/test/test_compute_coverage_path.cpp index f3614fa4..5bfed600 100644 --- a/opennav_coverage_bt/test/test_compute_coverage_path.cpp +++ b/opennav_coverage_bt/test/test_compute_coverage_path.cpp @@ -191,6 +191,40 @@ TEST_F(ComputeCoveragePathActionTestFixture, test_decomp_ports) tree_->haltTree(); } +TEST_F(ComputeCoveragePathActionTestFixture, test_tsp_ports) +{ + // B1-T14: route_mode_type="TSP" and tsp_* ports flow from BT XML to goal + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + + const auto & goal = action_server_->getReceivedGoal(); + EXPECT_EQ(goal.route_mode.mode, std::string("TSP")); + EXPECT_FALSE(goal.route_mode.tsp_redirect_swaths); + EXPECT_EQ(goal.route_mode.tsp_time_limit, 3u); + EXPECT_TRUE(goal.route_mode.tsp_search_for_optimum); + EXPECT_NEAR(goal.route_mode.tsp_d_tol, 0.0005, 1e-9); + + tree_->haltTree(); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From cb3902ad3e57adab3f60b0c7b692f70935208d5d Mon Sep 17 00:00:00 2001 From: u33549 Date: Thu, 16 Jul 2026 12:03:15 +0300 Subject: [PATCH 7/7] Declare ament_index_cpp dependency in coverage navigator The navigator used ament_index_cpp::get_package_share_directory without declaring the dependency or including its header, relying on transitive includes. ament_index_cpp 1.14 (rolling/lyrical) removed that header, breaking the build with "get_package_share_directory is not a member of ament_index_cpp". Add the explicit dependency in package.xml and CMakeLists, and include the header in both the source and the test. --- opennav_coverage_navigator/CMakeLists.txt | 3 +++ opennav_coverage_navigator/package.xml | 1 + opennav_coverage_navigator/src/coverage_navigator.cpp | 1 + opennav_coverage_navigator/test/test_coverage_navigator.cpp | 1 + 4 files changed, 6 insertions(+) diff --git a/opennav_coverage_navigator/CMakeLists.txt b/opennav_coverage_navigator/CMakeLists.txt index 24259db3..84fae291 100644 --- a/opennav_coverage_navigator/CMakeLists.txt +++ b/opennav_coverage_navigator/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(opennav_coverage_navigator) find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) @@ -24,6 +25,7 @@ include_directories( ) set(dependencies + ament_index_cpp rclcpp rclcpp_action std_msgs @@ -38,6 +40,7 @@ set(dependencies ) set(dep_targets + ament_index_cpp::ament_index_cpp rclcpp::rclcpp rclcpp_action::rclcpp_action ${std_msgs_TARGETS} diff --git a/opennav_coverage_navigator/package.xml b/opennav_coverage_navigator/package.xml index d9c77d33..e930041d 100644 --- a/opennav_coverage_navigator/package.xml +++ b/opennav_coverage_navigator/package.xml @@ -9,6 +9,7 @@ ament_cmake + ament_index_cpp rclcpp rclcpp_action nav2_behavior_tree diff --git a/opennav_coverage_navigator/src/coverage_navigator.cpp b/opennav_coverage_navigator/src/coverage_navigator.cpp index 679d72d4..f942473d 100644 --- a/opennav_coverage_navigator/src/coverage_navigator.cpp +++ b/opennav_coverage_navigator/src/coverage_navigator.cpp @@ -16,6 +16,7 @@ #include #include #include +#include "ament_index_cpp/get_package_share_directory.hpp" #include "opennav_coverage_navigator/coverage_navigator.hpp" namespace opennav_coverage_navigator diff --git a/opennav_coverage_navigator/test/test_coverage_navigator.cpp b/opennav_coverage_navigator/test/test_coverage_navigator.cpp index c7a3012d..114b070b 100644 --- a/opennav_coverage_navigator/test/test_coverage_navigator.cpp +++ b/opennav_coverage_navigator/test/test_coverage_navigator.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "gtest/gtest.h" +#include "ament_index_cpp/get_package_share_directory.hpp" #include "rclcpp/rclcpp.hpp" #include "opennav_coverage_navigator/coverage_navigator.hpp" #include "nav2_ros_common/lifecycle_node.hpp"