Add preemption guard for small areas

Author: mertgulerx

include/frontier_exploration_ros2/frontier_search.hpp

@@ -219,6 +219,7 @@ std::optional<VisibleRevealGain> compute_visible_reveal_gain(
   const std::optional<OccupancyGrid2d> & local_costmap,
   double range_m,
   double fov_deg,
-  double ray_step_deg);
+  double ray_step_deg,
+  const std::optional<FrontierCandidate::CellBounds> & visible_reveal_bounds = std::nullopt);
 
 }  // namespace frontier_exploration_ros2

include/frontier_exploration_ros2/frontier_types.hpp

@@ -266,6 +266,14 @@ class FrontierCache
 // Frontier output used by policy and goal dispatch.
 struct FrontierCandidate
 {
+  struct CellBounds
+  {
+    int min_x{0};
+    int min_y{0};
+    int max_x{0};
+    int max_y{0};
+  };
+
   FrontierCandidate() = default;
 
   // Compatibility constructor for existing nearest-style tests and simple callers.
@@ -288,14 +296,16 @@ struct FrontierCandidate
     std::pair<int, int> start_cell_in,
     std::pair<double, double> start_world_point_in,
     std::optional<std::pair<double, double>> goal_point_in,
-    int size_in)
+    int size_in,
+    std::optional<CellBounds> visible_reveal_bounds_in = std::nullopt)
   : centroid(std::move(centroid_in)),
     center_point(std::move(center_point_in)),
     center_cell(std::move(center_cell_in)),
     start_cell(std::move(start_cell_in)),
     start_world_point(std::move(start_world_point_in)),
     goal_point(std::move(goal_point_in)),
-    size(size_in)
+    size(size_in),
+    visible_reveal_bounds(std::move(visible_reveal_bounds_in))
   {
   }
 
@@ -313,6 +323,8 @@ struct FrontierCandidate
   std::optional<std::pair<double, double>> goal_point;
   // Number of cells in the underlying frontier cluster.
   int size{0};
+  // Conservative local bounds used to limit target-pose visible-reveal estimation around this cluster.
+  std::optional<CellBounds> visible_reveal_bounds;
 };
 
 enum class FrontierStrategy

src/frontier_explorer_core_dispatch.cpp

@@ -284,14 +284,26 @@ std::optional<double> FrontierExplorerCore::active_goal_visible_reveal_length()
   sensor_pose.orientation = detail::quaternion_from_yaw(heading + yaw_offset_rad);
 
   // The helper returns a local, occlusion-aware reveal length estimate around the target pose.
+  const auto visible_reveal_bounds =
+    std::visit(
+    [](const auto & frontier) -> std::optional<FrontierCandidate::CellBounds> {
+      using FrontierT = std::decay_t<decltype(frontier)>;
+      if constexpr (std::is_same_v<FrontierT, FrontierCandidate>) {
+        return frontier.visible_reveal_bounds;
+      }
+      return std::nullopt;
+    },
+    *active_goal_frontier);
+
   const auto visible_gain = compute_visible_reveal_gain(
     sensor_pose,
     *map,
     *costmap,
     local_costmap,
     params.goal_preemption_lidar_range_m,
     params.goal_preemption_lidar_fov_deg,
-    params.goal_preemption_lidar_ray_step_deg);
+    params.goal_preemption_lidar_ray_step_deg,
+    visible_reveal_bounds);
   if (!visible_gain.has_value()) {
     return std::nullopt;
   }
@@ -441,6 +453,15 @@ void FrontierExplorerCore::consider_preempt_active_goal(const std::string & trig
     if (!visible_reveal_length.has_value()) {
       callbacks.log_warn(
         "Skipping visible reveal gain gate for the active goal; falling back to frontier snapshot reselection");
+    } else if (
+      map.has_value() &&
+      static_cast<double>(frontier_size(*active_goal_frontier)) * map->map().info.resolution <=
+      *visible_reveal_length)
+    {
+      // Small frontier clusters can look fully revealable even with low absolute gain thresholds.
+      // If the current visible reveal already covers the whole cluster-size proxy, skip preemption.
+      reset_replacement_candidate_tracking();
+      return;
     } else if (*visible_reveal_length >= params.goal_preemption_lidar_min_reveal_length_m) {
       // Keep current goal while target pose can still reveal enough unexplored boundary on arrival.
       reset_replacement_candidate_tracking();

src/frontier_search.cpp

@@ -407,6 +407,23 @@ std::optional<FrontierCandidate> build_frontier_candidate(
     centroid_sum_y / static_cast<double>(new_frontier.size()),
   };
 
+  int min_frontier_x = new_frontier.front()->mapX;
+  int max_frontier_x = new_frontier.front()->mapX;
+  int min_frontier_y = new_frontier.front()->mapY;
+  int max_frontier_y = new_frontier.front()->mapY;
+  for (auto * frontier_point : new_frontier) {
+    min_frontier_x = std::min(min_frontier_x, frontier_point->mapX);
+    max_frontier_x = std::max(max_frontier_x, frontier_point->mapX);
+    min_frontier_y = std::min(min_frontier_y, frontier_point->mapY);
+    max_frontier_y = std::max(max_frontier_y, frontier_point->mapY);
+  }
+  const FrontierCandidate::CellBounds visible_reveal_bounds{
+    std::max(0, min_frontier_x - 1),
+    std::max(0, min_frontier_y - 1),
+    std::min(occupancy_map.getSizeX() - 1, max_frontier_x + 1),
+    std::min(occupancy_map.getSizeY() - 1, max_frontier_y + 1),
+  };
+
   std::pair<int, int> center_cell = {new_frontier.front()->mapX, new_frontier.front()->mapY};
   std::pair<double, double> center_point = frontier_world_points.front();
   double center_distance_sq = std::numeric_limits<double>::infinity();
@@ -454,6 +471,7 @@ std::optional<FrontierCandidate> build_frontier_candidate(
       start_world_point,
       std::nullopt,
       static_cast<int>(new_frontier.size()),
+      visible_reveal_bounds,
     };
   }
 
@@ -524,6 +542,7 @@ std::optional<FrontierCandidate> build_frontier_candidate(
     start_world_point,
     *goal_point,
     static_cast<int>(new_frontier.size()),
+    visible_reveal_bounds,
   };
 }
 
@@ -803,7 +822,8 @@ std::optional<VisibleRevealGain> compute_visible_reveal_gain(
   const std::optional<OccupancyGrid2d> & local_costmap,
   double range_m,
   double fov_deg,
-  double ray_step_deg)
+  double ray_step_deg,
+  const std::optional<FrontierCandidate::CellBounds> & visible_reveal_bounds)
 {
   // Invalid geometry means the caller should skip this optimization and keep the fallback path.
   if (range_m < 0.1 || fov_deg < 1.0 || fov_deg > 360.0 || ray_step_deg < 0.25 || ray_step_deg > 45.0) {
@@ -859,6 +879,15 @@ std::optional<VisibleRevealGain> compute_visible_reveal_gain(
         // Leaving the map bounds is equivalent to exhausting that ray.
         break;
       }
+      if (visible_reveal_bounds.has_value() &&
+        (map_x < visible_reveal_bounds->min_x ||
+        map_x > visible_reveal_bounds->max_x ||
+        map_y < visible_reveal_bounds->min_y ||
+        map_y > visible_reveal_bounds->max_y))
+      {
+        // Treat the local frontier-cluster bounds as a virtual wall for visible-gain estimation.
+        break;
+      }
 
       if (map_x == last_map_x && map_y == last_map_y) {
         continue;

test/test_frontier_snapshot_and_search.cpp

@@ -318,6 +318,42 @@ TEST(FrontierSearchTests, VisibleRevealGainRespectsYawAndFov)
   EXPECT_EQ(north_gain->visible_reveal_cell_count, 0);
 }
 
+TEST(FrontierSearchTests, VisibleRevealGainStopsAtVirtualClusterBounds)
+{
+  auto map_msg = build_grid(12, 12, -1);
+  for (int x = 2; x <= 4; ++x) {
+    set_cells(map_msg, {{x, 6}}, 0);
+  }
+
+  auto costmap_msg = build_grid(12, 12, 0);
+  const OccupancyGrid2d occupancy_map(map_msg);
+  const OccupancyGrid2d costmap(costmap_msg);
+
+  const auto unclipped_gain = compute_visible_reveal_gain(
+    make_pose(3.0, 6.0, 0.0),
+    occupancy_map,
+    costmap,
+    std::nullopt,
+    6.0,
+    1.0,
+    1.0);
+  const auto clipped_gain = compute_visible_reveal_gain(
+    make_pose(3.0, 6.0, 0.0),
+    occupancy_map,
+    costmap,
+    std::nullopt,
+    6.0,
+    1.0,
+    1.0,
+    FrontierCandidate::CellBounds{2, 6, 4, 6});
+
+  ASSERT_TRUE(unclipped_gain.has_value());
+  ASSERT_TRUE(clipped_gain.has_value());
+  EXPECT_GT(unclipped_gain->visible_reveal_cell_count, 0);
+  EXPECT_EQ(clipped_gain->visible_reveal_cell_count, 0);
+  EXPECT_DOUBLE_EQ(clipped_gain->visible_reveal_length_m, 0.0);
+}
+
 TEST(FrontierSearchTests, ExplorationCompleteCallbackRunsWhenNoFrontiersRemain)
 {
   auto core = make_snapshot_core();

test/test_preemption_flow.cpp

@@ -616,6 +616,54 @@ TEST(PreemptionFlowTests, VisibleGainGateSkipsSnapshotSearchWhenGoalStillHasVisi
   EXPECT_TRUE(core->pending_frontier_sequence.empty());
 }
 
+TEST(PreemptionFlowTests, VisibleGainGateSkipsSmallClusterEvenBelowMinRevealThreshold)
+{
+  auto core = make_preemption_core();
+  auto fake_handle = std::make_shared<FakeGoalHandle>();
+  core->goal_handle = fake_handle;
+  core->params.goal_preemption_enabled = true;
+  core->params.goal_preemption_lidar_range_m = 6.0;
+  core->params.goal_preemption_lidar_fov_deg = 90.0;
+  core->params.goal_preemption_lidar_ray_step_deg = 1.0;
+  core->params.goal_preemption_lidar_min_reveal_length_m = 2.0;
+  core->active_goal_frontier = FrontierCandidate{{4.0, 5.0}, {3.0, 5.0}, 1};
+  core->active_goal_frontiers = {*core->active_goal_frontier};
+
+  auto map_msg = build_grid(12, 12, -1);
+  for (int x = 0; x <= 4; ++x) {
+    set_cell(map_msg, x, 5, 0);
+  }
+  core->map = OccupancyGrid2d(map_msg);
+
+  int frontier_search_calls = 0;
+  int dispatch_calls = 0;
+  core->callbacks.wait_for_action_server = [](double) {return true;};
+  core->callbacks.dispatch_goal_request = [&dispatch_calls](const GoalDispatchRequest &) {
+      dispatch_calls += 1;
+    };
+  core->callbacks.frontier_search = [&frontier_search_calls](
+    const geometry_msgs::msg::Pose &,
+    const OccupancyGrid2d &,
+    const OccupancyGrid2d &,
+    const std::optional<OccupancyGrid2d> &,
+    double,
+    bool)
+    {
+      frontier_search_calls += 1;
+      FrontierSearchResult result;
+      result.frontiers = {FrontierCandidate{{8.0, 8.0}, {8.0, 8.0}, 8}};
+      result.robot_map_cell = {0, 0};
+      return result;
+    };
+
+  core->consider_preempt_active_goal("map");
+
+  EXPECT_EQ(frontier_search_calls, 0);
+  EXPECT_EQ(dispatch_calls, 0);
+  EXPECT_EQ(fake_handle->cancel_calls, 0);
+  EXPECT_TRUE(core->pending_frontier_sequence.empty());
+}
+
 TEST(PreemptionFlowTests, VisibleGainGateFallsBackToSnapshotReselectionWhenGoalGainIsExhausted)
 {
   auto core = make_preemption_core();