Add Bounded-Horizon Dynamic Programming ordering to MRTSP

Author: mertgulerx

CMakeLists.txt

@@ -33,6 +33,7 @@ add_library(${PROJECT_NAME}_core
   src/frontier_explorer_core_suppression.cpp
   src/frontier_explorer_core_dispatch.cpp
   src/mrtsp_ordering.cpp
+  src/mrtsp_solver.cpp
 )
 target_include_directories(${PROJECT_NAME}_core PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -213,6 +214,21 @@ if(BUILD_TESTING)
     nav_msgs
   )
 
+  ament_add_gtest(test_mrtsp_solver
+    test/test_mrtsp_solver.cpp
+  )
+  target_link_libraries(test_mrtsp_solver
+    ${PROJECT_NAME}_core
+  )
+  target_include_directories(test_mrtsp_solver PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}/include
+  )
+  ament_target_dependencies(test_mrtsp_solver
+    action_msgs
+    geometry_msgs
+    nav_msgs
+  )
+
   ament_add_gtest(test_control_service_and_idle
     test/test_control_service_and_idle.cpp
   )

README.md

@@ -18,9 +18,17 @@ frontier_explorer:
     autostart: true
     control_service_enabled: true
 
-    # Frontier Selection Strategy
+    # Frontier selection strategy. Use "mrtsp" for MRTSP ordering or "nearest" for distance policy.
     strategy: mrtsp
 
+    # MRTSP solver mode. "dp" scores all optimized frontiers, keeps the best candidate pool,
+    # and searches a bounded sequence; "greedy" traverses the full MRTSP matrix one step at a time.
+    mrtsp_solver: dp
+    # Maximum number of scored frontier candidates passed into the bounded-horizon solver.
+    dp_solver_candidate_limit: 15
+    # Number of distinct frontier visits evaluated in the candidate pool before dispatching sequence[0].
+    dp_planning_horizon: 10
+
     # QoS 
     map_qos_durability: transient_local
     map_qos_reliability: reliable

config/params.yaml

@@ -16,6 +16,7 @@ limitations under the License.
 
 #pragma once
 
+#include <cstddef>
 #include <functional>
 #include <memory>
 #include <optional>
@@ -81,6 +82,11 @@ struct FrontierExplorerCoreParams
   double weight_gain_ws{1.0};
   double max_linear_speed_vmax{0.5};
   double max_angular_speed_wmax{1.0};
+  // MRTSP can traverse its cost matrix greedily or with bounded dynamic programming.
+  std::string mrtsp_solver{"dp"};
+  // DP limits are algorithm-level controls, so they are not tied to one strategy name.
+  std::size_t dp_solver_candidate_limit{15};
+  std::size_t dp_planning_horizon{10};
   int occ_threshold{OCC_THRESHOLD};
   int min_frontier_size_cells{MIN_FRONTIER_SIZE};
   double frontier_candidate_min_goal_distance_m{0.0};
@@ -379,6 +385,11 @@ class FrontierExplorerCore
     double weight_gain_ws{0.0};
     double max_linear_speed_vmax{0.0};
     double max_angular_speed_wmax{0.0};
+    // Solver settings are part of the order cache key because they can change the
+    // selected sequence without any map, frontier, or robot-pose change.
+    std::string mrtsp_solver;
+    std::size_t dp_solver_candidate_limit{0};
+    std::size_t dp_planning_horizon{0};
     FrontierSequence frontier_sequence;
   };
   std::optional<MrtspOrderCacheEntry> mrtsp_order_cache;

include/frontier_exploration_ros2/frontier_explorer_core.hpp

@@ -75,6 +75,16 @@ double lower_bound_time_cost(
   double max_linear_speed_vmax,
   double max_angular_speed_wmax);
 
+// Computes the exact robot-start row cost used by build_cost_matrix().
+// Candidate pruning calls this helper to stay aligned with MRTSP matrix semantics.
+double compute_mrtsp_start_cost(
+  const FrontierCandidate & candidate,
+  const RobotState & robot_state,
+  const CostWeights & weights,
+  double sensor_effective_range_m,
+  double max_linear_speed_vmax,
+  double max_angular_speed_wmax);
+
 // Builds the directed MRTSP heuristic matrix consumed by the greedy ordering step.
 MrtspCostMatrix build_cost_matrix(
   const std::vector<FrontierCandidate> & frontiers,

include/frontier_exploration_ros2/mrtsp_ordering.hpp

@@ -0,0 +1,66 @@
+/*
+Copyright 2026 Mert Güler
+
+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.
+*/
+
+#pragma once
+
+#include <cstddef>
+#include <vector>
+
+#include "frontier_exploration_ros2/frontier_types.hpp"
+#include "frontier_exploration_ros2/mrtsp_ordering.hpp"
+
+namespace frontier_exploration_ros2
+{
+
+constexpr std::size_t kMaxDpSolverCandidateLimit{60U};
+
+struct MrtspSolverConfig
+{
+  // Upper bound for the candidate pool passed to the bounded solver.
+  std::size_t candidate_limit{15};
+  // Number of distinct frontier visits evaluated in each receding-horizon plan.
+  std::size_t planning_horizon{10};
+};
+
+struct MrtspPrunedCandidate
+{
+  // Index in the frontier vector received from the core. The DP solver works on the
+  // pruned vector, then callers use this field to map the result back to dispatch data.
+  std::size_t original_index{};
+  FrontierCandidate candidate{};
+  // MRTSP start-row cost used for deterministic top-N candidate pruning.
+  double score{0.0};
+};
+
+// Scores candidates with the same robot-to-frontier cost used by the MRTSP matrix,
+// sorts them deterministically, and returns the best bounded candidate pool.
+std::vector<MrtspPrunedCandidate> prune_mrtsp_candidates(
+  const std::vector<FrontierCandidate> & candidates,
+  const RobotState & robot_state,
+  const CostWeights & weights,
+  double sensor_effective_range_m,
+  double max_linear_speed_vmax,
+  double max_angular_speed_wmax,
+  const MrtspSolverConfig & config);
+
+// Searches the lowest-cost K-frontier sequence in the given matrix.
+// Matrix node 0 is the robot start node, and nodes 1..M are pruned frontiers.
+// Returned indices are relative to the pruned candidate list.
+std::vector<std::size_t> solve_bounded_horizon_mrtsp_order(
+  const MrtspCostMatrix & cost_matrix,
+  std::size_t planning_horizon);
+
+}  // namespace frontier_exploration_ros2

include/frontier_exploration_ros2/mrtsp_solver.hpp

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>frontier_exploration_ros2</name>
-  <version>1.4.1</version>
+  <version>1.5.0</version>
   <description>Modern C++ frontier exploration package for autonomous mobile robots, with a core C++ library for custom integrations alongside ROS 2 Jazzy / Nav2 integration.</description>
   <maintainer email="support.mertgulerx@gmail.com">mertgulerx</maintainer>
   <license>Apache-2.0</license>

package.xml

@@ -772,19 +772,52 @@ bool FrontierExplorerCore::send_frontier_goal(
   if (frontier_sequence.empty()) {
     return false;
   }
+
+  std::size_t dispatch_index = 0U;
+  for (; dispatch_index < frontier_sequence.size(); ++dispatch_index) {
+    const auto & candidate_frontier = frontier_sequence[dispatch_index];
+    if (!params.goal_skip_on_blocked_goal) {
+      break;
+    }
+
+    const auto cost_status = frontier_cost_status(std::optional<FrontierLike>{candidate_frontier});
+    if (!cost_status.has_value()) {
+      break;
+    }
+
+    callbacks.log_info(
+      "Skipping blocked frontier goal before dispatch: " + *cost_status +
+      "; " + describe_frontier(candidate_frontier));
+  }
+
+  if (dispatch_index >= frontier_sequence.size()) {
+    callbacks.log_info(
+      "All selected frontier goals are blocked before dispatch; waiting for updated costmap or frontier data");
+    return false;
+  }
+
+  FrontierSequence dispatch_sequence;
+  dispatch_sequence.reserve(frontier_sequence.size() - dispatch_index);
+  for (std::size_t index = dispatch_index; index < frontier_sequence.size(); ++index) {
+    dispatch_sequence.push_back(frontier_sequence[index]);
+  }
+
   const auto goal_pose = build_goal_pose(
-    frontier_sequence.front(),
+    dispatch_sequence.front(),
     current_pose);
   if (debug_outputs_enabled()) {
     callbacks.publish_selected_frontier_pose(goal_pose);
   }
+  const std::string dispatch_description = dispatch_index == 0U ?
+    description :
+    "Sending frontier goal after blocked-goal skip: " + describe_frontier(dispatch_sequence.front());
   // Frontier mode dispatches only the first element from the selected sequence.
   return send_pose_goal(
     goal_pose,
     "frontier",
-    frontier_sequence.front(),
-    frontier_sequence,
-    description);
+    dispatch_sequence.front(),
+    dispatch_sequence,
+    dispatch_description);
 }
 
 void FrontierExplorerCore::dispatch_goal_request(