diff --git a/opennav_coverage/CMakeLists.txt b/opennav_coverage/CMakeLists.txt index a35ce59..0de86b2 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/path_generator.hpp b/opennav_coverage/include/opennav_coverage/path_generator.hpp index b6d1feb..140ac71 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/route_generator.hpp b/opennav_coverage/include/opennav_coverage/route_generator.hpp index 361bbf9..dd7265e 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 0000000..5ed2095 --- /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/swath_generator.hpp b/opennav_coverage/include/opennav_coverage/swath_generator.hpp index 591d7b1..840b112 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/types.hpp b/opennav_coverage/include/opennav_coverage/types.hpp index 95ac6d4..7f7370a 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/include/opennav_coverage/utils.hpp b/opennav_coverage/include/opennav_coverage/utils.hpp index 7955463..4d6349b 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/coverage_server.cpp b/opennav_coverage/src/coverage_server.cpp index 591b55a..994f69a 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_coverage/src/path_generator.cpp b/opennav_coverage/src/path_generator.cpp index 2528cb9..54ffaa1 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/route_generator.cpp b/opennav_coverage/src/route_generator.cpp index dfd7cc1..e35499a 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 0000000..9e6c0d7 --- /dev/null +++ b/opennav_coverage/src/route_method.cpp @@ -0,0 +1,98 @@ +// 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); + + // 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 diff --git a/opennav_coverage/src/swath_generator.cpp b/opennav_coverage/src/swath_generator.cpp index ae1b621..b88bba9 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."); } diff --git a/opennav_coverage/test/CMakeLists.txt b/opennav_coverage/test/CMakeLists.txt index 53a1997..358846d 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 4e01189..7e01a07 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 57374f2..a582817 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 b545ed8..1fd2e42 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 5381e36..df3e1f2 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/include/opennav_coverage_bt/compute_complete_coverage_path.hpp b/opennav_coverage_bt/include/opennav_coverage_bt/compute_complete_coverage_path.hpp index 73280f8..e63b4d3 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 82502db..ad8613a 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)) { diff --git a/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp b/opennav_coverage_bt/test/test_cancel_complete_coverage.cpp index 9a6491b..e59937c 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 f3614fa..5bfed60 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); diff --git a/opennav_coverage_msgs/msg/RouteMode.msg b/opennav_coverage_msgs/msg/RouteMode.msg index e058b6b..3beb2cf 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) diff --git a/opennav_coverage_navigator/CMakeLists.txt b/opennav_coverage_navigator/CMakeLists.txt index 24259db..84fae29 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 d9c77d3..e930041 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 679d72d..f942473 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 c7a3012..114b070 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" diff --git a/opennav_row_coverage/src/row_coverage_server.cpp b/opennav_row_coverage/src/row_coverage_server.cpp index 1579889..2679a32 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 =