# Lightweight Planning

This package keeps the existing Nav2 minimal planner launch, and adds a lighter
fixed-field option:

1. Use a sparse topology graph for fixed venues.
2. Publish a densified `nav_msgs/Path`.
3. Track it with Pure Pursuit at a low, deterministic control cost.

The node is intended for cases where localization noise and runtime delay make
high-batch MPPI difficult to tune on the robot.

For random cone obstacles, the package includes a lightweight Ackermann Hybrid
A*/state-lattice planner implemented in C++. It consumes the fused robot pose,
target pose, OccupancyGrid costmap, and optional cone list, then publishes
`/plan` for the tracker. The executable name is kept as
`grid_astar_theta_node` for launch compatibility, but the planner is no longer
2D A*/Theta*.

## Run

```bash
colcon build --packages-select planner
source install/setup.bash
ros2 launch planner topology_pure_pursuit.launch.py
```

Send a graph-node target:

```bash
ros2 topic pub -1 /topology_goal std_msgs/msg/String "{data: bed_b}"
```

Or send `/goal_pose` near a graph node; the nearest node is selected.

## Random Obstacles / Ackermann Hybrid A*

Run the grid planner and tracker together:

```bash
ros2 launch planner grid_astar_theta_with_tracker.launch.py
```

Publish detected cone obstacles as a flat `Float32MultiArray`:

```bash
ros2 topic pub -1 /random_obstacles std_msgs/msg/Float32MultiArray \
  "{data: [1.2, 0.6, 0.15, 2.1, 1.4, 0.20]}"
```

By default, `/random_obstacles` is incremental. New cones are merged into the
stored cone set, and old cones remain. Detections within
`obstacle_merge_distance` are treated as the same cone and update its position.
Clear stored cones when needed:

```bash
ros2 topic pub -1 /clear_random_obstacles std_msgs/msg/Bool "{data: true}"
```

Send a target:

```bash
ros2 topic pub -1 /goal_pose geometry_msgs/msg/PoseStamped \
  "{header: {frame_id: map}, pose: {position: {x: 3.0, y: 1.5}, orientation: {w: 1.0}}}"
```

The planner also subscribes to `/global_costmap/costmap` as
`nav_msgs/OccupancyGrid`. Occupied cells (`>= 50`) and unknown cells (`-1`) are
blocked by default.

The planner publishes `/plan` as `nav_msgs/Path`; each pose orientation is the
vehicle body yaw. If the segment direction is opposite that yaw, the tracker
treats that segment as reverse and publishes negative `/cmd_vel.linear.x`.

## Files

- `config/topology_graph.yaml`: fixed-field topology graph. Replace the sample
  coordinates with measured centerline coordinates in the map frame.
- `config/topology_pure_pursuit.yaml`: tracking, delay compensation, and topic
  parameters.
- `launch/topology_pure_pursuit.launch.py`: standalone launch for the topology
  planner and Pure Pursuit tracker.
- `config/grid_astar_theta.yaml`: costmap fusion, cone inflation, and Ackermann
  Hybrid A* search parameters.
- `launch/grid_astar_theta_with_tracker.launch.py`: random-obstacle Ackermann
  planner plus yaw-aware tracker.
- `src/grid_astar_theta_node.cpp`: OccupancyGrid fusion, cone inflation,
  Hybrid A* search, and `/plan` publishing.
- `scripts/grid_astar_theta_node.py`: Python reference version kept for quick
  experiments; launch files use the C++ executable by default.
- `scripts/topology_pure_pursuit_node.py`: Dijkstra graph search, external
  `/plan` tracking, yaw-aware reverse handling, TF-aware pose handling, and
  latency compensation.

## Operating Notes

- Do not run this node's `/cmd_vel` output together with Nav2
  `controller_server`; choose one command publisher.
- If the graph is in `map` and odometry is in `odom`, keep TF available. The node
  transforms odometry poses into `map_frame`.
- Increase `nearest_node_max_distance`, `goal_tolerance`, and graph clearance
  when localization is noisy.
- Tune `lookahead_distance`, `min_turning_radius`, and `reverse_speed` first for
  Ackermann corner and reverse behavior.
- If the base expects Ackermann commands, run
  `scripts/ackermann_cmd_bridge.py` and configure the base driver
  accordingly.

## Planner Semantics

- Minimum turning radius defaults to `0.40 m`.
- `/goal_pose` uses `x`, `y`, and `yaw`; yaw tolerance defaults to 10 degrees.
- Replanning is immediate on new goals and limited to 2 Hz for costmap or
  accumulated cone updates.
- If new planning fails, the last collision-free path is kept; if it is no
  longer safe, an empty `/plan` is published so the tracker stops.
