Frontier exploration: our own WFD vs m-explore-ros2 (research)

Context

frontier_picker.py already contains a working skeleton of WFD + utility scoring (Topiwala 2018 + Gupta 2023 UT Austin). The research question is: keep our own, or migrate to an off-the-shelf ROS 2 package?

The honest reason this question exists at all is that “don’t reinvent the wheel” is a strong default in robotics — every custom component is a maintenance liability. But sometimes the existing wheels don’t fit your axle, and the analysis below walks through why that’s the case here.

Evaluation matrix (6 axes)

Candidate	Algorithm	Nav2 hookup	Jazzy	Sparse map	Maintenance	CPU	Σ
Our frontier_picker.py (WFD+utility) + EWFD patch	4★	5★ ActionClient	5★ rclpy native	5★ works with any /map Hz	5★ our own code	4★ Python BFS O(W·H)	28
m-explore-ros2 (main)	4★ explore_lite	3★ own goal publisher	3★ Humble target	3★ uses slam_gmapping (no ROS 2!)	3★ multi-robot focus	4★ C++	20
m-explore-ros2 (feature/slam_toolbox_compat)	4★	3★	2★ experimental	4★ slam_toolbox compat	2★ “active development”	4★	19
SeanReg/nav2_wfd	4★ WFD	3★ waypoint follower	1★ Foxy-only, 6 commits	4★ works	1★ stale	4★ Python	17
Nav2 BT plugin C++ (custom)	5★	5★ native BT plugin	5★	4★	1★ write from scratch	5★ C++	25

Winner: our own frontier_picker.py + EWFD patch + Nav2 ActionClient (28).

Final recommendation — what to add on top of the existing skeleton

1. EWFD optimization (Frontiers in Robotics 2021)

Use prev_frontier_cells as the BFS seed on the next tick. ~50% CPU savings per tick in steady state.

# In __init__:
self._prev_frontier_cells: set[tuple[int, int]] = set()

# In _wavefront_detect, before outer_q init:
seed_cells = [(rr, rc)] + list(self._prev_frontier_cells)
outer_q = deque(seed_cells)
visited_outer.update(seed_cells)

# At the end of _tick, after scoring:
self._prev_frontier_cells = set()
for f in frontiers:
    self._prev_frontier_cells.update(f.cells)

Reset on empty ticks. Cells are revalidated through 0 ≤ v ≤ FREE_MAX — if they’re no longer FREE, BFS skips them.

2. sensor_range_factor (Nature Sci Reports 2025)

Gaussian around SENSOR_OPTIMAL_RANGE = 4.0 m (TF-Luna 8 m / 2). Frontiers ≥ 8 m get a linear penalty; frontiers < 1 m get a flat −0.3 (already nearly scanned).

def _sensor_range_factor(self, dist: float) -> float:
    if dist < 1.0:    return -0.3
    if dist > 8.0:    return -(dist - 8.0) / 8.0
    return math.exp(-((dist - 4.0)**2) / (2 * 1.5**2))   # σ=1.5 m

# In utility scoring:
score = AREA * area_norm + DELTA_AREA * unknown_density - BETA_DIST * dist_norm \
        + DELTA_SENSOR * self._sensor_range_factor(dist)

This makes frontier selection “sweep range-aware” — we don’t chase a frontier we can’t reach in a single cycle.

3. exploration_done detection

When len(frontiers) == 0 after _wavefront_detect for three ticks in a row → publish /exploration/done (std_msgs/Bool data=True) → mission_fsm transitions to DONE.

self._empty_ticks_count = 0  # in __init__
# in _tick:
if not frontiers:
    self._empty_ticks_count += 1
    if self._empty_ticks_count >= EMPTY_TICKS_FOR_DONE:  # 3
        self.pub_done.publish(Bool(data=True))
else:
    if self._empty_ticks_count > 0:
        self.pub_done.publish(Bool(data=False))
    self._empty_ticks_count = 0

mission_fsm — Nav2 ActionClient hookup

from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose

class MissionFsm(Node):
    def __init__(self):
        super().__init__('mission_fsm')
        self.nav_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')
        self.create_subscription(PoseStamped, '/next_frontier', self._on_frontier, 10)
        self.create_subscription(Bool, '/exploration/done', self._on_done, 10)
        self.state = 'IDLE'

    def _on_frontier(self, goal_pose: PoseStamped):
        if self.state != 'IDLE': return
        self.state = 'MOVE'
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose = goal_pose
        self.nav_client.wait_for_server()
        future = self.nav_client.send_goal_async(
            goal_msg, feedback_callback=self._nav_feedback)
        future.add_done_callback(self._goal_response_cb)

    def _goal_result_cb(self, future):
        # Arrival at frontier — transition to SCAN
        self.state = 'SCAN'
        # ... trigger autoscan_node sweep

Fallback if Nav2 isn’t running (wait_for_server 2 s timeout) → transition to GOTO_FRONTIER without an active goal, plus a hover-based legacy path. There’s also a GOTO_TIMEOUT_S = 60.0 hard fallback.

Why NOT m-explore-ros2

• main branch requires slam_gmapping — we use slam_toolbox.
• feature/slam_toolbox_compat branch — experimental, “under active development.”
• Multi-robot focus — we have a SOLO drone.

Why NOT a custom Nav2 BT plugin (C++)

• Premature complexity for our mission_fsm cycle (IDLE → SCAN → MOVE).
• 1-2 days of work for marginal gain.
• Defer as a possible future refactor if mission_fsm grows into a complex BT.

GSoC 2023 drone gotchas (ArduPilot ROS 2)

Three lessons from GSoC 2023 GPS-Denied Exploration:

1. REP 105 TF violations: ros_gz transforms violate REP 105 → use robot_state_publisher for a correct TF tree.
2. Twist vs TwistStamped: Nav2 uses Twist, requiring body-frame velocity compatibility. Not our problem — we send NavigateToPose (PoseStamped goal); the Nav2 controller assembles velocity itself.
3. Cartographer-Costmap incompatibility — was a GSoC issue; slam_toolbox doesn’t reproduce it.

Sources

1. Topiwala 2018 WFD baseline — https://arxiv.org/pdf/1806.03581
2. Gupta et al. 2023 UT Austin utility-augmented — https://arxiv.org/abs/2309.14150
3. Frontiers in Robotics 2021 — Survey EWFD/FFD/WFD — https://www.frontiersin.org/articles/10.3389/frobt.2021.616470/full
4. Nature Sci Reports 2025 — Real-time map optimization + frontier cost function — https://www.nature.com/articles/s41598-025-97231-9
5. GSoC 2023 ArduPilot ROS 2 GPS-denied — https://discuss.ardupilot.org/t/.../101121
6. Nav2 NavigateToPose docs — action interface