Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 36 additions & 0 deletions autonomy/ros2/docker/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,42 @@ installs `lhr_vehicle`'s mesh into `share/`, and that is what makes the car's
`package://` URI resolve. Skipping it gives a broken-asset error rather than
an older car.

## Demo runs of the full Gazebo stack

`demo.sh` runs the gate stack in Gazebo with the GUI on the noVNC desktop
and the Foxglove bridge on `ws://localhost:8765`, for showing the car to
people rather than scoring it. It needs the container to be built (above);
it starts colima and the container if they are stopped.

```bash
cd autonomy/ros2/docker
./demo.sh lap # autocross seed 1, stops by itself after the finish
./demo.sh fault # oval at 8 m/s, steer_freeze trips STEER_TRACKING, car stops
./demo.sh loop # rules_autocross seed 1, laps until stopped
./demo.sh extreme # #218's extreme_combined course, laps until stopped
./demo.sh fsi # #223's real FSI layout (fsg for FSG), laps until stopped
./demo.sh fsi paused # car on the line, sim paused; ./demo.sh resume to go
./demo.sh pause | resume | stop
```

The terminal prints state changes, faults, trips and lap scores. The
Gazebo camera follows the car; double-click the window's title bar to
maximize it. In Foxglove, import
[`lhr_gazebo_truth.json`](../foxglove/README.md#truth-view) to follow
the car's true pose instead of the EKF's.

- Gazebo renders on the CPU here, so runs go at about half real time, and
about a quarter with Foxglove subscribed to the camera. Sim time drives
everything, so the runs are the same, only slower.
- `extreme`, `fsi` and `fsg` fetch their worlds from the unmerged
[#218](https://github.com/LonghornRacingElectric/lhre/pull/218) and
[#223](https://github.com/LonghornRacingElectric/lhre/pull/223) branches
and run them on this checkout's stack. Until those merge, `world_viz` on
`main` cannot draw the imported FSI/FSG layouts (no truth cones in
Foxglove), and the FSG car went off course after 9 m in #223's own check.
- `lap` is the only one with a scored pass on a Mac (33.6 s, 0 cones,
11/15). The looping runs turn metrics off, so nothing scores them.

After a reboot: `colima start && docker start lhr-autonomy`. If the
`Dockerfile` or `compose.yaml` changed since the image was built (e.g. after a
pull), `docker start` resumes the stale image, so run `docker compose up -d
Expand Down
105 changes: 105 additions & 0 deletions autonomy/ros2/docker/demo.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,105 @@
#!/usr/bin/env bash
# Run this on the MAC, like foxglove.sh. Demo runs of the full Gazebo stack:
# the Gazebo GUI on the noVNC desktop, the Foxglove bridge on
# ws://localhost:8765, and a truth frame for ../foxglove/lhr_gazebo_truth.json.
#
# ./demo.sh lap autocross seed 1, stops by itself after the finish
# ./demo.sh fault oval at 8 m/s; steer_freeze fires at 7.6 m/s, safety stops the car
# ./demo.sh loop rules_autocross seed 1, laps until stopped
# ./demo.sh extreme #218's extreme_combined course, laps until stopped
# ./demo.sh fsi #223's real FSI layout, laps until stopped
# ./demo.sh fsg #223's real FSG layout, laps until stopped
# ./demo.sh <run> paused start with the car on the line and the sim paused
# ./demo.sh pause | resume | stop
#
# Gazebo: http://localhost:6080/vnc.html?autoconnect=true&resize=scale
# Ctrl-C only ends the event printout; the sim runs until "stop".
set -e
export PATH=/opt/homebrew/bin:/usr/local/bin:$PATH

CONTAINER=lhr-autonomy
HERE=$(cd "$(dirname "$0")" && pwd)
REPO=$(git -C "$HERE" rev-parse --show-toplevel)
# The gate stack (testing/tools/gate/stack.args). Gazebo on a Mac renders on
# the CPU and drops LiDAR frames, hence the documented 1.5 s timeout (#160).
STACK="perception:=lidar estimator:=ekf safety:=true lidar_source:=mid360 \
enable_stereo_camera:=true enable_camera_lidar_fusion:=true safety_lidar_timeout_s:=1.5"

in_container() { docker exec -u ubuntu "$CONTAINER" bash -c "source /opt/ros/jazzy/setup.bash; $1"; }

world_control() { # pause: true | pause: false
in_container "gz service -s /world/fsae_world/control --reqtype gz.msgs.WorldControl \
--reptype gz.msgs.Boolean --timeout 1000 --req '$1'" 2>/dev/null | grep -q true
}

stop_sim() { # also waits for the old world to exit, so "paused" pauses the new one
docker exec "$CONTAINER" bash -c 'pkill -INT -f "[r]os2 launch lhr_gazebo"
pkill -f "[t]ruth_relay.py"
for i in $(seq 1 15); do pgrep -f "[g]z sim|[r]os2 launch" >/dev/null || exit 0; sleep 1; done
pkill -KILL -f "[g]z sim"; true' || true
}

pr_world() { # branch path: a world from an unmerged PR, copied to the container's /tmp
git -C "$REPO" fetch -q origin "$1"
git -C "$REPO" show "FETCH_HEAD:$2" | docker exec -i "$CONTAINER" sh -c "cat > /tmp/$(basename "$2")"
}

WORLDS=autonomy/ros2/src/lhr_gazebo/worlds
case "$1" in
lap) ARGS="track_style:=autocross seed:=1" ;;
fault) ARGS="track_style:=oval seed:=1 v_max:=8.0 enable_metrics:=false \
fault:=steer_freeze fault_after_s:=10 fault_min_speed_mps:=7.6" ;;
# Metrics off: nothing ends the mission at the finish, so the car keeps lapping.
loop) ARGS="track_style:=rules_autocross seed:=1 world:=/tmp/rules_autocross_seed1.sdf enable_metrics:=false" ;;
# Until #218 merges: its 3 m corridor, 80 m straight into a 9 m hairpin,
# slalom, chicane and nearby return lane, on main's stack.
extreme) ARGS="track_style:=rules_autocross seed:=1 width_m:=3.0 world:=/tmp/extreme_combined_seed1.sdf enable_metrics:=false" ;;
# Until #223 merges: main has no amz_* track style, so the legacy style
# stands in, whose width_m is cone-center spacing (the layouts' median gate
# width). max_step_m:=8 is #223's diagnostic override for FSG's sparse gates.
fsi) ARGS="track_style:=autocross seed:=1 width_m:=3.92 max_step_m:=8 world:=/tmp/amz_fsi.sdf enable_metrics:=false" ;;
fsg) ARGS="track_style:=autocross seed:=1 width_m:=3.96 max_step_m:=8 world:=/tmp/amz_fsg.sdf enable_metrics:=false" ;;
pause) world_control 'pause: true' && echo paused; exit ;;
resume) world_control 'pause: false' && echo running; exit ;;
stop) stop_sim; echo stopped; exit 0 ;;
*) sed -n '2,15p' "$0" >&2; exit 1 ;;
esac

colima status >/dev/null 2>&1 || colima start
[ "$(docker inspect -f '{{.State.Running}}' "$CONTAINER")" = true ] || docker start "$CONTAINER" >/dev/null

case "$1" in
loop) in_container "cd ~/autonomy/ros2 && source install/setup.bash && python3 \
src/lhr_gazebo/scripts/generate_world.py --style rules_autocross --seed 1 \
--output /tmp/rules_autocross_seed1.sdf >/dev/null" ;;
extreme) pr_world tanush/extreme-autocross "$WORLDS/extreme_combined_seed1.sdf" ;;
fsi) pr_world tanush/competition-layouts "$WORLDS/amz_fsi.sdf" ;;
fsg) pr_world tanush/competition-layouts "$WORLDS/amz_fsg.sdf" ;;
esac

stop_sim
docker exec -d -u ubuntu -e DISPLAY=:1 -e LIBGL_ALWAYS_SOFTWARE=1 "$CONTAINER" bash -lc \
"cd ~/autonomy/ros2 && exec ./scripts/run_gazebo_demo.sh gui:=true rviz:=false foxglove:=true $STACK $ARGS > /tmp/demo.log 2>&1"
docker exec -d -u ubuntu "$CONTAINER" bash -lc \
"source ~/autonomy/ros2/install/setup.bash; exec python3 ~/autonomy/ros2/foxglove/truth_relay.py > /tmp/truth_relay.log 2>&1"
echo "starting $1 ..."

if [ "$2" = paused ]; then
for i in $(seq 1 240); do world_control 'pause: true' && break; sleep 0.25; done
echo "paused on the line: ./demo.sh resume, or play in Gazebo"
sleep 10 # the GUI and the car exist well before sim time would reach READY
else
for i in $(seq 1 120); do docker exec "$CONTAINER" grep -q "OFF -> READY" /tmp/demo.log 2>/dev/null && break; sleep 1; done
fi

# Harmonic follows through the /gui/track topic; the /gui/follow service
# answers true and does nothing.
in_container "gz topic -t /gui/track -m gz.msgs.CameraTrack -p 'track_mode: FOLLOW_LOOK_AT, \
follow_target: {name: \"fsae_vehicle\"}, track_target: {name: \"fsae_vehicle\"}, \
follow_offset: {x: -8, y: 0, z: 4}, follow_pgain: 0.2, track_pgain: 0.2'" \
|| echo "camera follow failed: right-click fsae_vehicle in the Entity Tree > Follow"
echo "running (double-click the Gazebo title bar to maximize it)"

[ -n "$NOTAIL" ] && exit 0
docker exec "$CONTAINER" bash -c 'tail -n +1 -F /tmp/demo.log | grep --line-buffered -E \
"State: |Fault start|Safety trip|lap_time_s|cones_hit|max_cte|process has died"'
18 changes: 18 additions & 0 deletions autonomy/ros2/foxglove/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,24 @@ scorer ends; mission state and speed continue to show the final stop. Truth
cones use `/lhr/viewer/cones_truth`; the fixed reference path uses
`/lhr/viewer/centerline_truth`. Neither feeds perception or planning.

## Truth view

`lhr_gazebo.json` follows `base_link`, which the EKF owns, so the view moves
with every estimator correction. `lhr_gazebo_truth.json` is the same layout
following `truth_base_link`, Gazebo's true pose, with the car body drawn
there and the steering plot showing both front wheels' measured angles
against the command. Gazebo publishes the true pose on `/tf_truth`, which
Foxglove does not read as a frame, so
[`truth_relay.py`](https://github.com/LonghornRacingElectric/lhre/blob/main/autonomy/ros2/foxglove/truth_relay.py)
republishes it on `/tf` as `map -> truth_base_link`, and the body markers on
that frame. It is display-only: nothing in the stack reads `truth_base_link`.
[`docker/demo.sh`](../docker/README.md#demo-runs-of-the-full-gazebo-stack)
starts it with every run; by hand, inside the container:

```bash
python3 ~/autonomy/ros2/foxglove/truth_relay.py
```

## Record and check the layout

Gazebo has no `record:=true` argument. While it runs, use another sourced
Expand Down
Loading
Loading