path_planner: Graph Model, BFS, and Route Stitching

The current path_planner gives fleet_manager a route that is simple,
stable, and explainable enough to close the task assignment loop.

The planner’s first goal is not to be sophisticated. Its first goal is to be
minimally correct.

This article focuses on three questions:

1. why the current system uses a discrete waypoint graph;
2. why BFS is chosen instead of a more complex method;
3. why a route is split into start -> pickup and pickup -> dropoff before
   being stitched together.

Current Goal

The planner has a narrow responsibility in V1:

1. receive a start waypoint;
2. receive a pickup waypoint;
3. receive a dropoff waypoint;
4. return a traversable sequence of waypoints in the configured graph.

It is not doing continuous trajectory planning, dynamic obstacle avoidance, or
global multi-robot planning.

In the current architecture, the planner translates task semantics into
discrete route semantics. It does not attempt to solve the entire robot motion
problem.

Service Contract

The service interface is PlanRoute.srv:

1
2
3
4
5
6
7

string start_waypoint
string pickup_waypoint
string dropoff_waypoint
---
bool success
string message
string[] route_waypoints

This interface is not a generic graph search API. It carries transport-task
semantics directly.

The planner is not only asked:

“How do I go from A to B?”

It is asked:

“How do I go from the current start to pickup, and then from pickup to dropoff?”

That means the planner understands the current task model. The manager decides
which robot should take the task, and the planner converts that task into a
route.

Why a Discrete Graph Is Used First

The demo map is defined by two concepts:

1. a waypoint list;
2. an edge list.

Example waypoints include:

• dock_a;
• mid_1;
• pickup_zone;
• mid_2;
• dropoff_zone;
• dock_c.

Example edges include:

• dock_a:mid_1;
• mid_1:pickup_zone;
• mid_1:mid_2;
• mid_2:dropoff_zone;
• mid_2:dock_c.

This models the environment as an unweighted discrete graph.

That is reasonable because V1 is validating the fleet coordination path, not
the full expressiveness of a map representation.

At this stage, the manager only needs to know:

1. where the robot is symbolically located;
2. where pickup is;
3. where dropoff is;
4. whether a connected route exists.

By using a discrete graph, the project intentionally avoids high-complexity
topics such as continuous mapping, local obstacle avoidance, trajectory timing,
kinematic constraints, and richer map integration. This is stage control, not a
permanent limitation.

Parameterized Graph Configuration

The graph is not hard-coded directly into the planner logic. The planner reads
parameters such as:

• graph_waypoints;
• graph_edges.

Then load_graph_from_parameters() builds the internal adjacency list.

This separates planning logic from map content. The planner hard-codes the
model shape:

1. a graph consists of waypoints and edges;
2. each edge is represented as from:to;
3. the internal representation is an adjacency list.

The actual map can come from launch configuration. That gives the project
useful properties:

• the demo graph can change without modifying planner code;
• the same planner can serve multiple small maps;
• documentation can discuss algorithm behavior separately from scenario
  configuration.

One important boundary is that each configured edge is currently treated as
bidirectional. After reading from:to, the planner adds both from -> to and
to -> from. That is fine for a demo, but one-way aisles, restricted
directions, and weighted edges will require a graph model upgrade.

Why BFS Is Chosen

It is natural to ask why the planner is not Dijkstra, A*, or a full navigation
system.

For the current assumptions, BFS is exactly the right tool:

1. waypoints are discrete;
2. edges have no weights;
3. the system only needs a shortest path by number of hops.

Under those conditions, BFS gives the smallest correct implementation. It is
stable, deterministic, easy to inspect, and sufficient for the V1 coordination
loop.

This is not a contest for the most advanced algorithm. It is an engineering
choice: choose the smallest correct method for the current problem model.

What BFS Does Here

The current shortest_path(start, goal) logic is standard:

1. verify that start and goal exist in the graph;
2. traverse the graph with a queue;
3. record each visited node’s parent;
4. stop when the goal is reached;
5. reconstruct the route by walking parents backward from the goal;
6. reverse the result into forward order.

The engineering value is in the behavior.

First, the result is a minimum-hop route in the current unweighted graph.

Second, failure is easy to interpret. If no parent entry is found for the goal,
the function returns an empty path.

Third, no heuristic assumptions are introduced. The route is determined by the
graph structure, not by extra scoring logic.

For this stage, predictability matters more than appearing smarter.

Why the Route Is Not Just start -> dropoff

The planner does not directly compute:

start_waypoint -> dropoff_waypoint

Instead, it computes:

1. start_waypoint -> pickup_waypoint;
2. pickup_waypoint -> dropoff_waypoint.

Then it appends the second segment to the first while skipping the duplicate
pickup node at the beginning of the second segment.

This matches the semantics of a transport task.

Pickup is not an optional midpoint. It is part of the task contract. If the
planner only searched start -> dropoff, it could choose a geometrically valid
route that bypasses pickup and is therefore semantically wrong.

In this task model, pickup is a hard constraint, not an optimization hint.

Skipping the first node of the second segment also matters. Without that step,
the returned route would contain pickup twice. Using logic equivalent to
std::next(to_dropoff.begin()) keeps the route clean for downstream execution.

That means the planner is already formatting the route for the agent instead of
forcing the agent to understand route stitching.

Why Explainability Matters

The current planner’s biggest strength is not only that it searches a graph. It
is that its output is easy to explain.

For a route such as:

dock_a -> mid_1 -> pickup_zone -> mid_1 -> mid_2 -> dropoff_zone

a developer can immediately answer:

1. it goes to pickup first;
2. then it goes to dropoff;
3. the intermediate waypoints correspond to graph connectivity;
4. the route follows the current task model.

That is valuable for debugging, logging, and documentation. A system that is
easy to explain is easier to validate incrementally.

What the Planner Does Not Solve Yet

Understanding a module also means understanding what it deliberately does not
solve.

The current planner has no edge weights. The route is shortest by hop count,
not by travel time, distance, or energy cost.

It has no conflict awareness. It checks whether one robot can traverse the
graph, not whether multiple robots can do so safely at the same time.

It has no dynamic replanning. The planner returns a route once; it does not
continuously adjust during execution.

It has no direction constraints because edges are treated as bidirectional.

It has no continuous-space semantics. It returns waypoint sequences, not real
trajectories or low-level navigation commands.

In other words, the planner is enough to validate discrete coordination-layer
correctness. It is not a complete navigation layer.

Where This Leads Next

After path_planner, the next natural question is how robot_agent consumes
route_waypoints, advances through them, and turns the route sequence into
idle, executing, and completed state transitions.

The higher-level follow-up questions are also clear:

1. if conflict reservation is added, should the planner interface change?
2. if edge weights are added, should BFS become Dijkstra or A*?
3. if Nav2 is integrated later, which layer should this planner occupy?

The current planner is small, but it sits at the right architectural point. It
is simple enough to reason about, and important enough to shape the next stage
of the system.

Summary

path_planner is not trying to perform complex route planning yet. It reliably
translates a transport task into a discrete, explainable, executable waypoint
route. That is exactly what the V1 task loop needs.

• ← Previous Post
• Next Post →