TurtleBotJEPA-PPO: ready-to-run JEPA + PPO policy for a real TurtleBot3 (LDS-02), guide incl. mapping and map goals

4421dc3 verified 4 days ago

15.1 kB

---
license: mit
library_name: pytorch
pipeline_tag: reinforcement-learning
tags:
- robotics
- turtlebot3
- ros2
- reinforcement-learning
- ppo
- jepa
- lidar
- collision-avoidance
- navigation
base_model: penji2803/lidar-vjepa-turtlebot3
datasets:
- penji2803/turtlebot3-moving-obstacles-lidar-1.5m
---
# TurtleBotJEPA-PPO: run a JEPA + PPO navigation policy on a real TurtleBot3

Download this repository to a laptop, follow the guide below, and a **TurtleBot3 Burger with an LDS-02 lidar** drives to
places you click on a map of your room, avoiding obstacles. Everything runs on the laptop; the robot only needs the
standard ROBOTIS software.

```
 robot (bringup) ── /scan, /odom ──► laptop ──────────────────────────────────────────────────────── /cmd_vel ──► robot
                                       │ LDS-02 cleaning → last 20 scans → frozen LiDAR V-JEPA encoder → 1024 numbers
                                       │ → risk / clearance heads (8 directions, next 1 s) → 16 numbers
                                       │ goal distance + angle + 3 × 24 front beams + 16 ──► PPO policy ──► 5 turn rates
                                       └ collision shield (learned risk + footprint check) + hard safety stops
```

| Part | Where it comes from |
|---|---|
| LiDAR encoder | [penji2803/lidar-vjepa-turtlebot3](https://huggingface.co/penji2803/lidar-vjepa-turtlebot3): self-supervised V-JEPA-style model, frozen |
| Training data | [penji2803/turtlebot3-moving-obstacles-lidar-1.5m](https://huggingface.co/datasets/penji2803/turtlebot3-moving-obstacles-lidar-1.5m) |
| Policy | PPO (sb3-contrib MaskablePPO), trained in Gazebo, then fine-tuned with a simulated LDS-02 sensor model |

## How good is it? Read this first

| Test | Result |
|---|---|
| Simulation, 200 fixed episodes, 4 moving obstacles, LDS-02 sensor model | 38.5 % goals reached, the rest collisions |
| Gazebo rehearsal of this exact program, 8 trips, static pillars, half speed | 7 / 8 goals, 1 safety stop |
| Real TurtleBot3 Burger (LDS-02) | Early tests only: it reaches simple goals. The hard safety stop ended runs where the learned risk missed a box; the shield was then hardened. |

**It is a research model and is not safe for unattended use.** Rules:
- **Space:** use a clear, flat area of at least 3 × 3 m.
- **Obstacles:** soft ones only (cardboard, foam), **taller than 20 cm**; the lidar plane is about 18 cm high.
- **Supervision:** never let it drive at people or pets; keep a hand on **Ctrl+C**.
- **Last resort:** pick the robot up or switch it off.

## What you need

- **Robot:** a TurtleBot3 **Burger** with an **LDS-02** lidar (shipped 2022–2025; the model is printed on the sensor), a Raspberry Pi 4 and a charged battery.
  - LDS-01 and LDS-03 are untested; the code reads any lidar format, but the model was tuned for LDS-02.
- **Laptop:** Ubuntu 22.04 with ROS 2 Humble (no GPU needed).
- **Network:** one Wi-Fi network that both the laptop and the robot can join. A phone hotspot or home router works best; university Wi-Fi often blocks ROS 2.

---

## Part A: Laptop setup (once)

1. **Install ROS 2 Humble** by following the [official guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html) (the `ros-humble-desktop` variant). Then install the TurtleBot3, mapping and localization packages:
```bash
sudo apt install ros-humble-turtlebot3 ros-humble-turtlebot3-msgs ros-humble-turtlebot3-cartographer ros-humble-turtlebot3-navigation2 ros-humble-turtlebot3-teleop ros-humble-turtlebot3-gazebo ros-humble-cartographer-ros ros-humble-navigation2 ros-humble-nav2-bringup python3-pip
```
2. **Download this repository** into `~/TurtleBotJEPA-PPO`:
```bash
pip install -U huggingface_hub
```
```bash
hf download penji2803/TurtleBotJEPA-PPO --local-dir ~/TurtleBotJEPA-PPO
```
3. **Install the Python packages** for the system Python that ROS 2 uses. CPU-only PyTorch is enough:
```bash
pip install --user torch --index-url https://download.pytorch.org/whl/cpu
```
```bash
pip install --user -r ~/TurtleBotJEPA-PPO/requirements.txt
```
4. **Check the install** (no robot needed):
```bash
cd ~/TurtleBotJEPA-PPO && python3 tests/test_adapter.py
```
   It should end with `ALL TESTS PASSED`.

## Part B: Robot setup (once)

1. **Prepare the Raspberry Pi** with the ROBOTIS image for ROS 2 Humble and connect it to your Wi-Fi: see the ROBOTIS [SBC setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setup/) and [OpenCR setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/opencr_setup/) guides. Note the robot's IP address.
2. **Log in to the robot** from the laptop (the ROBOTIS default password is `turtlebot`):
```bash
ssh ubuntu@ROBOT_IP
```
3. **On the robot, set the lidar model, robot model and channel** (once):
```bash
echo 'export LDS_MODEL=LDS-02' >> ~/.bashrc
```
```bash
echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
```
```bash
echo 'export ROS_DOMAIN_ID=30' >> ~/.bashrc
```
   `ROS_DOMAIN_ID` is the "channel" laptop and robot talk on; it must be the same on both (this repo's default is 30).

## Part C: Try it without a robot (optional)

This needs a 360-ray simulated Burger. The ROBOTIS default Gazebo Burger is 360 rays; if you changed it, for example for the ROBOTIS DQN tutorial, restore it.

**Terminal 1:** start Gazebo:
```bash
bash ~/TurtleBotJEPA-PPO/tools/sim_demo.sh
```
**Terminal 2:** drive 1 m ahead in the simulated world:
```bash
cd ~/TurtleBotJEPA-PPO && source setup_terminal.sh sim && python3 run_policy.py --goal 1.0 0.0 --simulate-lds02
```
`--simulate-lds02` makes the clean simulated lidar behave like an LDS-02. Use it only in simulation.

---

## Part D: Start of every session

1. **Switch the robot on** and wait about 1 minute.
2. **Terminal A:** start the robot's bringup and leave it running for the whole session:
```bash
ssh ubuntu@ROBOT_IP
```
```bash
ros2 launch turtlebot3_bringup robot.launch.py
```
3. **In every new laptop terminal**, run this first:
```bash
cd ~/TurtleBotJEPA-PPO && source setup_terminal.sh
```
   It prints `Ready: ROS 2 humble, ROS_DOMAIN_ID=30, TURTLEBOT3_MODEL=burger ...`.
4. **Pre-flight check** (the robot does not move):
```bash
python3 check_robot.py
```
   - All lines should say `OK`.
   - Hold a box about 30 cm in front of the robot: **AHEAD** must drop. Then on its left: **LEFT** must drop.
   - If left and right are swapped, check `LDS_MODEL` on the robot and restart the bringup.

## Part E: Map your room (once per room)

**Terminal B:** start mapping; RViz opens and the map grows as the robot moves:
```bash
ros2 launch turtlebot3_cartographer cartographer.launch.py
```
**Terminal C:** drive the robot by hand. Keys: `w`/`x` faster or slower, `a`/`d` turn, `s` stop; the terminal must have focus.
```bash
ros2 run turtlebot3_teleop teleop_keyboard
```
**Mapping tips:**
- **Go slowly:** about 0.1 m/s, and turn slowly.
- **Close the loop:** drive around the whole room and come back to where you started.
- **Finish when the walls in RViz look solid.**

**Terminal D:** save the map:
```bash
mkdir -p ~/maps && ros2 run nav2_map_server map_saver_cli -f ~/maps/my_room
```
Then press **Ctrl+C** in terminals B and C.

## Part F: Click places on the map, the robot drives there

**Terminal B:** start localization on your map (map server + AMCL only; it never drives the robot):
```bash
ros2 launch nav2_bringup localization_launch.py map:=$HOME/maps/my_room.yaml params_file:=$(ros2 pkg prefix turtlebot3_navigation2)/share/turtlebot3_navigation2/param/burger.yaml use_sim_time:=False
```

**Terminal C:** open RViz:
```bash
rviz2 -d rviz/map_goals.rviz
```
**Terminal D:** start the driving program:
```bash
python3 map_goals.py
```
**Then, in RViz:**
1. **Tell it where the robot is:** click **"2D Pose Estimate"**, click where the robot really is on the map, and drag in the direction it faces. The red laser dots must line up with the walls; if not, do it again.
2. **Send it somewhere:** use **"Publish Point"** (one click) or **"2D Goal Pose"** to click a place. The robot drives there at half speed and then waits for the next click.
3. **Change your mind:** click a new place at any time to replace the goal.
4. **Stop:** press **Ctrl+C** in terminal D.

**Rules for map goals:**
- **Localization must be confident:** the robot refuses a goal while AMCL is unsure of its position (it prints why).
- **Goal placement:** choose goals 0.8–3 m away and at least 0.5 m from walls and furniture.
- **The policy doesn't plan routes.** It heads for the goal and avoids what its lidar sees; to get around a wall or into another room, click intermediate points (the doorway first).
- **Don't start `navigation2.launch.py`:** Nav2's own driver would fight this program for the wheels.

## Part G: Or a goal measured from the start (no map needed)

Put the robot on a taped start mark facing "ahead". The goal is X m ahead and Y m to the **left** (negative = right).
1. **Dry run** (the wheels never move):
```bash
python3 run_policy.py --goal 1.0 0.0 --dry-run
```
2. **Real run:**
```bash
python3 run_policy.py --goal 1.5 0.5
```

## Part H: Check the learned collision risk on your robot (strongly recommended)

The risk predictions were learned in simulation. These tools measure, and if needed adapt, them on your real sensor, using the robot's own future scans as labels (no manual labelling).
1. **Record 10 minutes** of driving by hand near (not into) boxes and walls:
```bash
python3 real_heads_check.py record --minutes 10
```
   While it records, drive with `ros2 run turtlebot3_teleop teleop_keyboard` in another terminal.
2. **Evaluate:**
```bash
python3 real_heads_check.py eval "logs/record_*.npz"
```
   You want **GO**: front-direction AUROC ≥ 0.90.
   - **NO-GO with enough close approaches:** add `--finetune`, then copy `policy/heads_real.safetensors` over `policy/heads.safetensors` (keep a backup).
   - The footprint shield and the safety stops work either way.

## Shutting down

1. **Ctrl+C** in all laptop terminals, then in terminal A (the bringup).
2. **On the robot:**
```bash
sudo shutdown now
```
3. Wait about 30 s, then switch the power off.

---

## Options

| Option (`run_policy.py` and `map_goals.py`) | Default | Meaning |
|---|---|---|
| `--speed-scale` | 0.5 | 0.5 = 0.10 m/s and half turn rates, with decision rate and lidar frame timing slowed to match (the policy sees the same geometry); 1.0 = training speed 0.20 m/s; 0.3 = slower |
| `--geo-horizon` | 1.2 | seconds of path the footprint shield checks (1.5 = turns away earlier) |
| `--geo-radius` | 0.17 | the shield blocks paths passing closer than this (m) to any lidar point |
| `--shield-tau` | 0.6 | learned-risk threshold of the shield (0.8 = less cautious) |
| `--safety-front` / `--safety-any` | 0.22 / 0.17 | hard stop distances (m): ahead ±30° / any direction |
| `--angle-offset` | 0 | rotate the lidar data (radians), e.g. 3.14159 if front and back are swapped |
| `--stamped` | off | the robot runs ROS 2 Jazzy (TwistStamped `cmd_vel`) |
| `--dry-run` (`run_policy.py`) | off | never move the wheels |
| `ROS_DOMAIN_ID` (environment) | 30 | `TB3_DOMAIN_ID=31 source setup_terminal.sh` if your robot uses another channel |

Logs: `logs/run_*.csv` / `logs/mapgoal_*.csv` (every decision) and `.npz` (the cleaned lidar frames).

## How it works

- **LDS-02 → training format** (`lds02.py`, `lds02_adapter.py`):
  - every scan, whatever its point count or direction, is mapped onto the simulator's 360-ray grid;
  - invalid readings (the LDS-02 sees nothing below 0.16 m, and reports 0 or inf) become "no return" (3.5 m), unless an obstacle was just seen close by (near-field hold);
  - ranges are clipped to 0.12–3.5 m;
  - unit tests check that this is identical to the training pipeline.
- **Policy input:** the goal distance and angle, the last 3 front scans (24 beams), and 8 risk + 8 clearance values from the JEPA heads.
- **Collision shield:** an action is blocked if
  - the learned risk of the directions it heads into (the highest value over ~0.6 s) is above 0.6, or
  - the arc the robot would drive over the next 1.2 s passes within 0.17 m of any lidar point (footprint check).
- **Hard safety stops**, independent of the network: something closer than 0.22 m ahead (±30°) or 0.17 m anywhere, stale sensor data (> 0.5 s), timeout, Ctrl+C.
- **Speed scaling:** speeds and timing are scaled together, so at half speed the robot moves the same 2 cm between decisions as in training.

## Troubleshooting

| Problem | Fix |
|---|---|
| `KeyError: 'TURTLEBOT3_MODEL'` | Run `source setup_terminal.sh` in that terminal, or `echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc` |
| `NO SENSOR DATA`, or `ros2 topic list` shows no `/scan` / `/odom` | The bringup isn't running, or `ROS_DOMAIN_ID` differs between laptop and robot, or the Wi-Fi blocks ROS 2 traffic (try a phone hotspot) |
| Left and right swapped in `check_robot.py` | `LDS_MODEL` on the robot doesn't match the sensor; fix it and restart the bringup |
| Frequent `SAFETY STOP` | Goals too close to obstacles, or approach too head-on; try `--geo-horizon 1.5`, goals further from obstacles, or `--speed-scale 0.3` |
| `AMCL is unsure where the robot is` | Use "2D Pose Estimate" again; drive a little with teleop so AMCL can match the walls |
| Robot circles near the goal | Click a goal a little further away or at a different angle |
| `SENSORS LOST` | Wi-Fi drop-outs; move closer to the router |

## Limitations

- **Simulation-trained:** one arena with cylinders. Real rooms, furniture and people are different; expect collisions to be stopped by the shield and safety stops rather than avoided gracefully.
- **No route planning:** the policy is reactive.
- **Learned risk:** the JEPA risk predictions were unreliable on a real box in our tests; do Part H.
- **Sensor tuning:** the LDS-02 behaviour was modelled from the datasheet; LDS-01 and LDS-03 are untested.

## Files

| File | What it is |
|---|---|
| `map_goals.py` | Click goals in RViz (AMCL) and the robot drives there |
| `run_policy.py` | One goal relative to the start pose |
| `check_robot.py`, `real_heads_check.py` | Pre-flight check; real-data check and fine-tuning of the risk heads |
| `lds02.py`, `lds02_adapter.py`, `v6_policy.py`, `tb3_jepa_agent.py`, `tb3_policy_core.py` | ROS-free core (numpy + torch + safetensors) |
| `policy/` | Policy, heads, frozen JEPA encoder (all safetensors) and `policy_config.json` |
| `rviz/map_goals.rviz`, `setup_terminal.sh`, `tools/sim_demo.sh`, `tests/` | RViz layout, terminal setup, Gazebo demo, unit tests |
| `media/gazebo_demo.mp4` | A Gazebo-rendered run (an earlier version of this policy, in simulation) |

## License and credits

MIT. Built on ROBOTIS [TurtleBot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/overview/) (ROS 2 packages, Stage 3
world, reward design). The encoder follows V-JEPA (Bardes et al., 2024), adapted to 2-D lidar.

	---
	license: mit
	library_name: pytorch
	pipeline_tag: reinforcement-learning
	tags:
	- robotics
	- turtlebot3
	- ros2
	- reinforcement-learning
	- ppo
	- jepa
	- lidar
	- collision-avoidance
	- navigation
	base_model: penji2803/lidar-vjepa-turtlebot3
	datasets:
	- penji2803/turtlebot3-moving-obstacles-lidar-1.5m
	---

	# TurtleBotJEPA-PPO: run a JEPA + PPO navigation policy on a real TurtleBot3

	Download this repository to a laptop, follow the guide below, and a TurtleBot3 Burger with an LDS-02 lidar drives to
	places you click on a map of your room, avoiding obstacles. Everything runs on the laptop; the robot only needs the
	standard ROBOTIS software.

	```
	robot (bringup) ── /scan, /odom ──► laptop ──────────────────────────────────────────────────────── /cmd_vel ──► robot
	│ LDS-02 cleaning → last 20 scans → frozen LiDAR V-JEPA encoder → 1024 numbers
	│ → risk / clearance heads (8 directions, next 1 s) → 16 numbers
	│ goal distance + angle + 3 × 24 front beams + 16 ──► PPO policy ──► 5 turn rates
	└ collision shield (learned risk + footprint check) + hard safety stops
	```

	\| Part \| Where it comes from \|
	\|---\|---\|
	\| LiDAR encoder \| [penji2803/lidar-vjepa-turtlebot3](https://huggingface.co/penji2803/lidar-vjepa-turtlebot3): self-supervised V-JEPA-style model, frozen \|
	\| Training data \| [penji2803/turtlebot3-moving-obstacles-lidar-1.5m](https://huggingface.co/datasets/penji2803/turtlebot3-moving-obstacles-lidar-1.5m) \|
	\| Policy \| PPO (sb3-contrib MaskablePPO), trained in Gazebo, then fine-tuned with a simulated LDS-02 sensor model \|

	## How good is it? Read this first

	\| Test \| Result \|
	\|---\|---\|
	\| Simulation, 200 fixed episodes, 4 moving obstacles, LDS-02 sensor model \| 38.5 % goals reached, the rest collisions \|
	\| Gazebo rehearsal of this exact program, 8 trips, static pillars, half speed \| 7 / 8 goals, 1 safety stop \|
	\| Real TurtleBot3 Burger (LDS-02) \| Early tests only: it reaches simple goals. The hard safety stop ended runs where the learned risk missed a box; the shield was then hardened. \|

	It is a research model and is not safe for unattended use. Rules:
	- Space: use a clear, flat area of at least 3 × 3 m.
	- Obstacles: soft ones only (cardboard, foam), taller than 20 cm; the lidar plane is about 18 cm high.
	- Supervision: never let it drive at people or pets; keep a hand on Ctrl+C.
	- Last resort: pick the robot up or switch it off.

	## What you need

	- Robot: a TurtleBot3 Burger with an LDS-02 lidar (shipped 2022–2025; the model is printed on the sensor), a Raspberry Pi 4 and a charged battery.
	- LDS-01 and LDS-03 are untested; the code reads any lidar format, but the model was tuned for LDS-02.
	- Laptop: Ubuntu 22.04 with ROS 2 Humble (no GPU needed).
	- Network: one Wi-Fi network that both the laptop and the robot can join. A phone hotspot or home router works best; university Wi-Fi often blocks ROS 2.

	---

	## Part A: Laptop setup (once)

	1. Install ROS 2 Humble by following the [official guide](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debs.html) (the `ros-humble-desktop` variant). Then install the TurtleBot3, mapping and localization packages:
	```bash
	sudo apt install ros-humble-turtlebot3 ros-humble-turtlebot3-msgs ros-humble-turtlebot3-cartographer ros-humble-turtlebot3-navigation2 ros-humble-turtlebot3-teleop ros-humble-turtlebot3-gazebo ros-humble-cartographer-ros ros-humble-navigation2 ros-humble-nav2-bringup python3-pip
	```
	2. Download this repository into `~/TurtleBotJEPA-PPO`:
	```bash
	pip install -U huggingface_hub
	```
	```bash
	hf download penji2803/TurtleBotJEPA-PPO --local-dir ~/TurtleBotJEPA-PPO
	```
	3. Install the Python packages for the system Python that ROS 2 uses. CPU-only PyTorch is enough:
	```bash
	pip install --user torch --index-url https://download.pytorch.org/whl/cpu
	```
	```bash
	pip install --user -r ~/TurtleBotJEPA-PPO/requirements.txt
	```
	4. Check the install (no robot needed):
	```bash
	cd ~/TurtleBotJEPA-PPO && python3 tests/test_adapter.py
	```
	It should end with `ALL TESTS PASSED`.

	## Part B: Robot setup (once)

	1. Prepare the Raspberry Pi with the ROBOTIS image for ROS 2 Humble and connect it to your Wi-Fi: see the ROBOTIS [SBC setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/sbc_setup/) and [OpenCR setup](https://emanual.robotis.com/docs/en/platform/turtlebot3/opencr_setup/) guides. Note the robot's IP address.
	2. Log in to the robot from the laptop (the ROBOTIS default password is `turtlebot`):
	```bash
	ssh ubuntu@ROBOT_IP
	```
	3. On the robot, set the lidar model, robot model and channel (once):
	```bash
	echo 'export LDS_MODEL=LDS-02' >> ~/.bashrc
	```
	```bash
	echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc
	```
	```bash
	echo 'export ROS_DOMAIN_ID=30' >> ~/.bashrc
	```
	`ROS_DOMAIN_ID` is the "channel" laptop and robot talk on; it must be the same on both (this repo's default is 30).

	## Part C: Try it without a robot (optional)

	This needs a 360-ray simulated Burger. The ROBOTIS default Gazebo Burger is 360 rays; if you changed it, for example for the ROBOTIS DQN tutorial, restore it.

	Terminal 1: start Gazebo:
	```bash
	bash ~/TurtleBotJEPA-PPO/tools/sim_demo.sh
	```
	Terminal 2: drive 1 m ahead in the simulated world:
	```bash
	cd ~/TurtleBotJEPA-PPO && source setup_terminal.sh sim && python3 run_policy.py --goal 1.0 0.0 --simulate-lds02
	```
	`--simulate-lds02` makes the clean simulated lidar behave like an LDS-02. Use it only in simulation.

	---

	## Part D: Start of every session

	1. Switch the robot on and wait about 1 minute.
	2. Terminal A: start the robot's bringup and leave it running for the whole session:
	```bash
	ssh ubuntu@ROBOT_IP
	```
	```bash
	ros2 launch turtlebot3_bringup robot.launch.py
	```
	3. In every new laptop terminal, run this first:
	```bash
	cd ~/TurtleBotJEPA-PPO && source setup_terminal.sh
	```
	It prints `Ready: ROS 2 humble, ROS_DOMAIN_ID=30, TURTLEBOT3_MODEL=burger ...`.
	4. Pre-flight check (the robot does not move):
	```bash
	python3 check_robot.py
	```
	- All lines should say `OK`.
	- Hold a box about 30 cm in front of the robot: AHEAD must drop. Then on its left: LEFT must drop.
	- If left and right are swapped, check `LDS_MODEL` on the robot and restart the bringup.

	## Part E: Map your room (once per room)

	Terminal B: start mapping; RViz opens and the map grows as the robot moves:
	```bash
	ros2 launch turtlebot3_cartographer cartographer.launch.py
	```
	Terminal C: drive the robot by hand. Keys: `w`/`x` faster or slower, `a`/`d` turn, `s` stop; the terminal must have focus.
	```bash
	ros2 run turtlebot3_teleop teleop_keyboard
	```
	Mapping tips:
	- Go slowly: about 0.1 m/s, and turn slowly.
	- Close the loop: drive around the whole room and come back to where you started.
	- Finish when the walls in RViz look solid.

	Terminal D: save the map:
	```bash
	mkdir -p ~/maps && ros2 run nav2_map_server map_saver_cli -f ~/maps/my_room
	```
	Then press Ctrl+C in terminals B and C.

	## Part F: Click places on the map, the robot drives there

	Terminal B: start localization on your map (map server + AMCL only; it never drives the robot):
	```bash
	ros2 launch nav2_bringup localization_launch.py map:=$HOME/maps/my_room.yaml params_file:=$(ros2 pkg prefix turtlebot3_navigation2)/share/turtlebot3_navigation2/param/burger.yaml use_sim_time:=False
	```

	Terminal C: open RViz:
	```bash
	rviz2 -d rviz/map_goals.rviz
	```
	Terminal D: start the driving program:
	```bash
	python3 map_goals.py
	```
	Then, in RViz:
	1. Tell it where the robot is: click "2D Pose Estimate", click where the robot really is on the map, and drag in the direction it faces. The red laser dots must line up with the walls; if not, do it again.
	2. Send it somewhere: use "Publish Point" (one click) or "2D Goal Pose" to click a place. The robot drives there at half speed and then waits for the next click.
	3. Change your mind: click a new place at any time to replace the goal.
	4. Stop: press Ctrl+C in terminal D.

	Rules for map goals:
	- Localization must be confident: the robot refuses a goal while AMCL is unsure of its position (it prints why).
	- Goal placement: choose goals 0.8–3 m away and at least 0.5 m from walls and furniture.
	- The policy doesn't plan routes. It heads for the goal and avoids what its lidar sees; to get around a wall or into another room, click intermediate points (the doorway first).
	- Don't start `navigation2.launch.py`: Nav2's own driver would fight this program for the wheels.

	## Part G: Or a goal measured from the start (no map needed)

	Put the robot on a taped start mark facing "ahead". The goal is X m ahead and Y m to the left (negative = right).
	1. Dry run (the wheels never move):
	```bash
	python3 run_policy.py --goal 1.0 0.0 --dry-run
	```
	2. Real run:
	```bash
	python3 run_policy.py --goal 1.5 0.5
	```

	## Part H: Check the learned collision risk on your robot (strongly recommended)

	The risk predictions were learned in simulation. These tools measure, and if needed adapt, them on your real sensor, using the robot's own future scans as labels (no manual labelling).
	1. Record 10 minutes of driving by hand near (not into) boxes and walls:
	```bash
	python3 real_heads_check.py record --minutes 10
	```
	While it records, drive with `ros2 run turtlebot3_teleop teleop_keyboard` in another terminal.
	2. Evaluate:
	```bash
	python3 real_heads_check.py eval "logs/record_*.npz"
	```
	You want GO: front-direction AUROC ≥ 0.90.
	- NO-GO with enough close approaches: add `--finetune`, then copy `policy/heads_real.safetensors` over `policy/heads.safetensors` (keep a backup).
	- The footprint shield and the safety stops work either way.

	## Shutting down

	1. Ctrl+C in all laptop terminals, then in terminal A (the bringup).
	2. On the robot:
	```bash
	sudo shutdown now
	```
	3. Wait about 30 s, then switch the power off.

	---

	## Options

	\| Option (`run_policy.py` and `map_goals.py`) \| Default \| Meaning \|
	\|---\|---\|---\|
	\| `--speed-scale` \| 0.5 \| 0.5 = 0.10 m/s and half turn rates, with decision rate and lidar frame timing slowed to match (the policy sees the same geometry); 1.0 = training speed 0.20 m/s; 0.3 = slower \|
	\| `--geo-horizon` \| 1.2 \| seconds of path the footprint shield checks (1.5 = turns away earlier) \|
	\| `--geo-radius` \| 0.17 \| the shield blocks paths passing closer than this (m) to any lidar point \|
	\| `--shield-tau` \| 0.6 \| learned-risk threshold of the shield (0.8 = less cautious) \|
	\| `--safety-front` / `--safety-any` \| 0.22 / 0.17 \| hard stop distances (m): ahead ±30° / any direction \|
	\| `--angle-offset` \| 0 \| rotate the lidar data (radians), e.g. 3.14159 if front and back are swapped \|
	\| `--stamped` \| off \| the robot runs ROS 2 Jazzy (TwistStamped `cmd_vel`) \|
	\| `--dry-run` (`run_policy.py`) \| off \| never move the wheels \|
	\| `ROS_DOMAIN_ID` (environment) \| 30 \| `TB3_DOMAIN_ID=31 source setup_terminal.sh` if your robot uses another channel \|

	Logs: `logs/run_.csv` / `logs/mapgoal_.csv` (every decision) and `.npz` (the cleaned lidar frames).

	## How it works

	- LDS-02 → training format (`lds02.py`, `lds02_adapter.py`):
	- every scan, whatever its point count or direction, is mapped onto the simulator's 360-ray grid;
	- invalid readings (the LDS-02 sees nothing below 0.16 m, and reports 0 or inf) become "no return" (3.5 m), unless an obstacle was just seen close by (near-field hold);
	- ranges are clipped to 0.12–3.5 m;
	- unit tests check that this is identical to the training pipeline.
	- Policy input: the goal distance and angle, the last 3 front scans (24 beams), and 8 risk + 8 clearance values from the JEPA heads.
	- Collision shield: an action is blocked if
	- the learned risk of the directions it heads into (the highest value over ~0.6 s) is above 0.6, or
	- the arc the robot would drive over the next 1.2 s passes within 0.17 m of any lidar point (footprint check).
	- Hard safety stops, independent of the network: something closer than 0.22 m ahead (±30°) or 0.17 m anywhere, stale sensor data (> 0.5 s), timeout, Ctrl+C.
	- Speed scaling: speeds and timing are scaled together, so at half speed the robot moves the same 2 cm between decisions as in training.

	## Troubleshooting

	\| Problem \| Fix \|
	\|---\|---\|
	\| `KeyError: 'TURTLEBOT3_MODEL'` \| Run `source setup_terminal.sh` in that terminal, or `echo 'export TURTLEBOT3_MODEL=burger' >> ~/.bashrc` \|
	\| `NO SENSOR DATA`, or `ros2 topic list` shows no `/scan` / `/odom` \| The bringup isn't running, or `ROS_DOMAIN_ID` differs between laptop and robot, or the Wi-Fi blocks ROS 2 traffic (try a phone hotspot) \|
	\| Left and right swapped in `check_robot.py` \| `LDS_MODEL` on the robot doesn't match the sensor; fix it and restart the bringup \|
	\| Frequent `SAFETY STOP` \| Goals too close to obstacles, or approach too head-on; try `--geo-horizon 1.5`, goals further from obstacles, or `--speed-scale 0.3` \|
	\| `AMCL is unsure where the robot is` \| Use "2D Pose Estimate" again; drive a little with teleop so AMCL can match the walls \|
	\| Robot circles near the goal \| Click a goal a little further away or at a different angle \|
	\| `SENSORS LOST` \| Wi-Fi drop-outs; move closer to the router \|

	## Limitations

	- Simulation-trained: one arena with cylinders. Real rooms, furniture and people are different; expect collisions to be stopped by the shield and safety stops rather than avoided gracefully.
	- No route planning: the policy is reactive.
	- Learned risk: the JEPA risk predictions were unreliable on a real box in our tests; do Part H.
	- Sensor tuning: the LDS-02 behaviour was modelled from the datasheet; LDS-01 and LDS-03 are untested.

	## Files

	\| File \| What it is \|
	\|---\|---\|
	\| `map_goals.py` \| Click goals in RViz (AMCL) and the robot drives there \|
	\| `run_policy.py` \| One goal relative to the start pose \|
	\| `check_robot.py`, `real_heads_check.py` \| Pre-flight check; real-data check and fine-tuning of the risk heads \|
	\| `lds02.py`, `lds02_adapter.py`, `v6_policy.py`, `tb3_jepa_agent.py`, `tb3_policy_core.py` \| ROS-free core (numpy + torch + safetensors) \|
	\| `policy/` \| Policy, heads, frozen JEPA encoder (all safetensors) and `policy_config.json` \|
	\| `rviz/map_goals.rviz`, `setup_terminal.sh`, `tools/sim_demo.sh`, `tests/` \| RViz layout, terminal setup, Gazebo demo, unit tests \|
	\| `media/gazebo_demo.mp4` \| A Gazebo-rendered run (an earlier version of this policy, in simulation) \|

	## License and credits

	MIT. Built on ROBOTIS [TurtleBot3](https://emanual.robotis.com/docs/en/platform/turtlebot3/overview/) (ROS 2 packages, Stage 3
	world, reward design). The encoder follows V-JEPA (Bardes et al., 2024), adapted to 2-D lidar.