# Real Robot Custom Hybrid A* Runbook

The dedicated launch file is `real_hybrid_astar.launch.py`. It starts only the
nodes required by the custom stack:

`origincar_base -> EKF -> AMCL/map_server -> LSLIDAR -> grid_astar_theta_node -> Pure Pursuit -> Ackermann bridge`

Nav2 `controller_server` is intentionally not started. Pure Pursuit is the only
node publishing `/cmd_vel`.

The repository also contains `planner/maps/real_5x5.yaml`, a synthetic empty
5x5 m room with boundary walls. Use it to validate the software chain only; it
must be replaced by a map created from the real environment before driving.

## Confirm before running

1. The map YAML and image are the real map. The repository example `zhihui` map is
   109x115 pixels at 0.05 m/pixel, or 5.45x5.75 m, not exactly 5x5 m.
2. The LSLIDAR model, interface and serial device match the selected parameter
   file. The example N10 configuration uses `/dev/ttyCH343USB0`.
3. The STM32 serial device and baud rate are correct. The default is
   `/dev/ttyACM0` and 115200.
4. The robot is Ackermann configured and the measured minimum turning radius is
   close to 0.40 m.
5. The initial robot pose in the map is known and the start cell is free.
6. The goal is position-only by default. Set `require_goal_yaw: true` in the
   planner parameters when a final docking heading is required.

## Build on the robot

```bash
cd ~/liao_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install \
  --packages-select \
  origincar_msg origincar_description origincar_base \
  lslidar_msgs lslidar_driver planner
source install/setup.bash
```

Before launching ROS, validate the map on any machine with Python and PyYAML:

```bash
python3 src/planner/scripts/map_info.py /actual/map.yaml
```

The check must report `PASS` for a 5x5 m map. Use `--no-check` only when the
measured map size is intentional and pass those dimensions to `preflight_check`.

## Start the stack

Use one terminal. Replace the map and lidar parameter paths when necessary.

```bash
ros2 launch planner real_hybrid_astar.launch.py \
  map:=$HOME/liao_ws/src/planner/maps/real_5x5.yaml \
  lidar_params:=$HOME/liao_ws/src/LSLIDAR_X_ROS2-20240228/src/lslidar_driver/params/lidar_uart_ros2/lsn10.yaml \
  lidar_serial_port:=/dev/ttyCH343USB0 \
  lidar_model:=N10 \
  start_lidar:=true \
  serial_port:=/dev/ttyACM0 \
  serial_baud:=115200 \
  enable_motion:=false
```

The command above uses the repository's synthetic 5x5 m room for a software
chain test. For real driving, replace `map:=...` with the YAML generated from
the actual environment and verify its dimensions and origin first.

Set `start_lidar:=false` when a standalone LSLIDAR launch already publishes
`/scan`; never let two driver processes open the same serial device.

Do not also launch `origincar_bringup`, Nav2 `navigation_launch`, another
controller, `path_follower_demo.py`, or `udp_to_cmdvel.py`; those can publish a
competing `/cmd_vel`.

## Verify interfaces

```bash
ros2 topic echo /scan --once
ros2 topic echo /map --once
ros2 topic echo /odom_combined --once
ros2 run tf2_ros tf2_echo map base_footprint
ros2 topic echo /hybrid_astar_status
ros2 topic echo /pure_pursuit_status
```

AMCL must be initialized before the preflight gate. Publish the measured initial
pose in the map:

```bash
ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped \
  "{header: {frame_id: map}, pose: {pose: {position: {x: 0.54, y: 0.20}, orientation: {w: 1.0}}, covariance: [0.25, 0, 0, 0, 0, 0, 0, 0.25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.10]}}"
```

Run the preflight gate before sending any goal. It checks the 5x5m map size,
`/scan`, `/odom_combined`, `map -> base_footprint`, and that exactly one node
publishes `/cmd_vel`. It also checks the `/ackermann_cmd` bridge and base
subscriber:

```bash
ros2 run planner preflight_check.py
```

For a deliberately different map size, override the expected dimensions rather
than ignoring the check:

```bash
ros2 run planner preflight_check.py --ros-args \
  -p expected_map_width:=5.45 -p expected_map_height:=5.75
```

## Send one goal

Start the status and `/plan` monitors in separate terminals before publishing;
these topics are not transient-local and an observer started later can miss the
message.

```bash
ros2 run planner send_goal.py 1.00 1.00
```

The optional third argument is the final yaw in radians. In the real-robot
configuration only x/y are required; the planner stops within the position
tolerance without forcing yaw to zero.

Equivalent raw command:

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

For the repository map, use a point strictly inside `[-2.5, 2.5)` and clear of
the inflated boundary walls. The `(1.0, 1.0)` example is valid; `(2.5, 2.5)` is
outside the usable grid.

The first launch uses `enable_motion:=false`, so it only validates planning.
After `/plan` and the steering direction have been checked, cancel the goal,
stop the launch, and restart with `enable_motion:=true` for low-speed driving.

Expected sequence:

1. `/hybrid_astar_status` reports `plan_ok`.
2. `/plan` contains the generated path.
3. With `enable_motion:=false`, the robot remains stopped for path inspection.
4. After restarting with `enable_motion:=true`, `/cmd_vel` is converted to
   `/ackermann_cmd` and the robot stops within `goal_tolerance` (0.15 m).

Cancel the active goal and stop with:

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

This clears the planner goal and tracker path. Publishing one zero `/cmd_vel`
message is not a reliable stop because the tracker can overwrite it at 20 Hz.

If AMCL localization is not available yet, do not drive autonomously. The
planner requires a valid `map -> odom` transform and recent `/odom_combined`.
