Upload folder using huggingface_hub

.dockerignore

@@ -0,0 +1,30 @@
+# colcon workspace artifacts (rebuilt inside the image)
+build/
+install/
+log/
+
+# git
+.git/
+.gitignore
+
+# python
+**/__pycache__/
+*.py[cod]
+*.egg-info/
+.pytest_cache/
+
+# runtime output, not source
+vla_logs/
+navila/result/
+
+# heavy weights (fetched separately, not committed) — small in-repo YOLO
+# weights under vlm/ are intentionally NOT excluded here, they ship with the image.
+navila-ckpt/
+*.safetensors
+*.pth
+
+# editor / misc
+.vscode/
+.DS_Store
+.superpowers/
+docs/

.gitattributes

@@ -33,3 +33,6 @@ saved_model/**/* filter=lfs diff=lfs merge=lfs -text
 *.zip filter=lfs diff=lfs merge=lfs -text
 *.zst filter=lfs diff=lfs merge=lfs -text
 *tfevents* filter=lfs diff=lfs merge=lfs -text
+.vscode/browse.vc.db filter=lfs diff=lfs merge=lfs -text
+go2_robot_sdk/dae/base.dae filter=lfs diff=lfs merge=lfs -text
+go2_robot_sdk/meshes/base.STL filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,32 @@
+# Python
+__pycache__/
+*.py[cod]
+*.egg-info/
+.pytest_cache/
+*.so
+
+# ROS build/install artifacts
+build
+log
+install
+.vscode
+.env
+.DS_Store
+go2_robot_sdk/.coverage
+go2_robot_sdk/coverage.xml
+
+# Runtime logs / dữ liệu sinh ra
+vla_logs/
+navila/result/
+*.log
+*.ply
+*.pgm
+*.pcd
+
+# Checkpoint / trọng số lớn (tải lại bằng script; KHÔNG đẩy lên git)
+navila-ckpt/
+*.safetensors
+*.pth
+
+.superpowers/
+docs/

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "go2_robot_sdk/external_lib/aioice"]
+	path = go2_robot_sdk/external_lib/aioice
+	url = https://github.com/legion1581/aioice.git
+	branch = go2

.vscode/browse.vc.db

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:356be10c121d584a032686b7ddb2b0916ede175104aa86b128b274e4e089b032
+size 1325277184

.vscode/c_cpp_properties.json

@@ -0,0 +1,22 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${workspaceFolder}/.vscode/browse.vc.db",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/thanhnc/unige_ws/install/spot_sar_msgs/include/**",
+        "/opt/ros/jazzy/include/**",
+        "/home/thanhnc/Go2-Nav_System/go2_navigation/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ros2",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++17"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,35 @@
+{
+    "ROS2.distro": "jazzy",
+    "python.autoComplete.extraPaths": [
+        "/home/thanhnc/unige_ws/build/spot_sar_sim",
+        "/home/thanhnc/unige_ws/install/spot_sar_sim/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_executive",
+        "/home/thanhnc/unige_ws/install/spot_sar_executive/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_planning",
+        "/home/thanhnc/unige_ws/install/spot_sar_planning/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_perception",
+        "/home/thanhnc/unige_ws/install/spot_sar_perception/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_nav",
+        "/home/thanhnc/unige_ws/install/spot_sar_nav/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/install/spot_sar_msgs/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_bringup",
+        "/home/thanhnc/unige_ws/install/spot_sar_bringup/lib/python3.12/site-packages",
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/thanhnc/unige_ws/build/spot_sar_sim",
+        "/home/thanhnc/unige_ws/install/spot_sar_sim/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_executive",
+        "/home/thanhnc/unige_ws/install/spot_sar_executive/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_planning",
+        "/home/thanhnc/unige_ws/install/spot_sar_planning/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_perception",
+        "/home/thanhnc/unige_ws/install/spot_sar_perception/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_nav",
+        "/home/thanhnc/unige_ws/install/spot_sar_nav/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/install/spot_sar_msgs/lib/python3.12/site-packages",
+        "/home/thanhnc/unige_ws/build/spot_sar_bringup",
+        "/home/thanhnc/unige_ws/install/spot_sar_bringup/lib/python3.12/site-packages",
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ]
+}

LICENSE

@@ -0,0 +1,24 @@
+BSD 2-Clause License
+
+Copyright (c) 2024, RoboVerse community
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -1,3 +1,243 @@
----
-license: bsd-2-clause
----
+# Unitree Go2 — ROS 2 Navigation, Mapping and Low-level Control & Basic Vision-Language-Action
+
+A ROS 2 workspace for the Unitree Go2 quadruped that pairs a classic navigation
+stack (SLAM Toolbox mapping + Nav2) with a **Vision-Language-Action (VLA)** layer:
+the robot can be driven by typing a natural-language instruction instead of a
+joystick or a Nav2 goal. Two interchangeable VLA brains are included — a local
+**Qwen2.5-VL** reasoning loop and **NaVILA** (VILA-8B), a model purpose-trained
+for language-guided navigation.
+
+|Systems|ROS2 distro|Build status|
+|--|--|--|
+|Ubuntu 24.04|jazzy|![ROS2 CI](https://github.com/abizovnuralem/go2_ros2_sdk/actions/workflows/ros_build.yaml/badge.svg)|
+
+
+## Repository map
+
+| Path | Role |
+|---|---|
+| [`go2_robot_sdk/`](go2_robot_sdk) | WebRTC/LAN driver — camera, odom, lidar, URDF |
+| [`go2_navigation/`](go2_navigation) | Bringup, mapping, Nav2, C++ lidar filtering pipeline |
+| [`go2_control/`](go2_control) | Keyboard teleop node |
+| [`go2_interfaces/`](go2_interfaces) | Shared ROS 2 message definitions |
+| [`vlm/`](vlm) | Local VLA stack: Qwen2.5-VL engine, YOLO-assisted servoing, web console |
+| [`navila/`](navila) | NaVILA (VILA-8B) inference server + setup/eval tooling (bare metal, not containerized) |
+| [`docker/`](docker) | Containerized driver + navigation + local VLA bringup |
+
+## Installation
+
+```shell
+mkdir -p ros2_ws
+cd ros2_ws
+git clone --recurse-submodules https://github.com/StrikeRobot/Go2-Nav_System src
+sudo apt install ros-$ROS_DISTRO-image-tools ros-$ROS_DISTRO-vision-msgs
+sudo apt install python3-pip clang portaudio19-dev
+cd src
+pip install -r requirements.txt
+cd ..
+source /opt/ros/$ROS_DISTRO/setup.bash
+rosdep install --from-paths src --ignore-src -r -y
+colcon build
+```
+
+## Docker
+
+```shell
+cd docker
+cp .env.example .env    # if you keep one locally; otherwise export ROBOT_IP inline
+ROBOT_IP=<go2-lan-ip> docker compose up go2                 # driver + Nav2 stack
+ROBOT_IP=<go2-lan-ip> docker compose --profile vla up vlm   # + local VLA web console (GPU)
+```
+The image builds on `ros:jazzy-ros-base`, matching the distro this project is
+developed against. `network_mode: host` is required for ROS 2 DDS discovery and
+for the driver's WebRTC session with the robot on the LAN. The `vlm` service
+requests a GPU via the [NVIDIA Container Toolkit](https://github.com/NVIDIA/nvidia-container-toolkit) —
+install that on the host first. **NaVILA is not containerized**: it needs its
+own conda environment, a hand-patched `transformers` build for Blackwell GPUs,
+and a ~16 GB checkpoint pulled at setup time, so it's run on bare metal via
+[`navila/setup_navila_blackwell.sh`](navila/setup_navila_blackwell.sh) — see the
+[NaVILA section](#brain-2--navila-vila-8b) below.
+
+Never bake a real `ROBOT_IP`/`ROBOT_TOKEN` into the image or into
+`go2_navigation/config/robot.yaml` — pass them as environment variables at run
+time instead.
+
+## Usage
+
+Put the Go2 in Wi-Fi/STA mode and note its LAN IP (mobile app → Device → Data →
+Automatic Machine Inspection → STA Network: `wlan0`). Set it once in
+[`go2_navigation/config/robot.yaml`](go2_navigation/config/robot.yaml) or pass
+`ROBOT_IP=<ip>` on any launch command (env always wins).
+
+```shell
+source install/setup.bash
+```
+
+### Mapping
+
+```shell
+ros2 launch go2_navigation mapping.launch.py
+```
+Mark a dock rectangle with tape as a fixed starting point. In the SlamToolbox
+panel in `rviz`, select **Start At Dock**, drive the robot around with a
+controller to build the map, then **Save Map** / **Serialize Map**.
+
+### Autonomous navigation (Nav2)
+
+```shell
+ros2 launch go2_navigation navigation.launch.py
+```
+Loads a saved map and drives the robot with Nav2's standard global/local
+planner stack + AMCL localization — set goals from `rviz` as usual.
+
+### Manual teleop
+
+```shell
+ros2 run go2_control keyboard_teleop
+```
+Arrows = move, `q`/`e` = rotate, space = stop, `x` = quit. Releasing all keys
+for >0.4 s triggers a dead-man stop.
+
+## VLA navigation
+
+Instead of a Nav2 goal, type an instruction and let a vision-language model
+decide the motion step by step from the live camera feed. **No RL or learned
+locomotion is involved anywhere in this stack** — every brain below only ever
+emits a discrete motion command (`move forward 25 cm`, `turn left 30°`,
+`stop`); the Go2's own built-in gait and this repo's closed-loop
+`MotionController` (odom + lidar + E-Stop) execute it.
+
+### Brain 1 — local Qwen2.5-VL (`vlm` package, "Demo1")
+
+```shell
+ros2 launch go2_navigation bringup.launch.py teleop:=true
+```
+```shell
+cd ros2_ws/src
+./vlm/scripts/run_demo1.sh
+```
+Open `http://localhost:8001`, type a goal in natural language (e.g. *"go to
+the water bottle, turn right, stop at the chair"*) or a manual command (`move
+forward 75 cm`, `turn left 90 deg`). Each step: camera frame → Qwen2.5-VL
+prompt (with live odom/lidar/bbox metrics injected as state) → JSON
+`{action, value, unit, is_finished, ...}` → `MotionController` executes →
+repeat until `is_finished` or **Stop/E-Stop**.
+
+Two control modes (`VLA_CONTROL`):
+- `vlm` (default) — the model reasons over injected state and returns the next action.
+- `servo` — continuous closed-loop visual servoing driven by YOLO detections,
+  bypassing the LLM once a target is locked on.
+
+Swap in a stronger model instead of the local 3B:
+```shell
+VLA_BRAIN=api VLA_API_URL=<endpoint> VLA_API_KEY=<key> VLA_MODEL=qwen2.5-vl-7b-instruct \
+  ./vlm/scripts/run_demo1.sh
+```
+
+<details>
+<summary>Configuration reference — local VLM brain</summary>
+
+| Env | Default | Meaning |
+|---|---|---|
+| `VLA_BRAIN` | `local` | `local` (Qwen2.5-VL-3B) \| `api` (OpenAI-compatible endpoint) \| `navila` |
+| `VLA_CONTROL` | `servo` | `servo` (visual servo, YOLO-driven) \| `vlm` (JSON reasoning loop) |
+| `USE_YOLO` | `1` | `0` disables the YOLO detector |
+| `VLA_MAX_STEPS` | `20` | max steps per instruction |
+| `VLA_MAX_TURN_DEG` | `10` | hard cap on turn angle per step (all brains) |
+| `VLA_SEARCH_MAX_DEG` | `360` | give up searching after this much cumulative scan rotation |
+| `VLA_HFOV_DEG` | `90` | camera horizontal FOV, used to convert pixel offset → angle |
+| `VLA_CENTER_TOL_DEG` | `5` | (servo) angular tolerance considered "centered" |
+| `VLA_SERVO_HZ` / `VLA_SERVO_VX` / `VLA_SERVO_WZ_MAX` / `VLA_SERVO_KP` | `20` / `0.25` / `0.175` / `1.5` | servo loop rate, forward speed, max turn rate, turn gain |
+| `VLA_STOP_BOTTOM_PX` | `20` | stop once the target bbox bottom is within this many px of the frame edge |
+| `MOTION_LIN_SPEED` / `MOTION_ANG_SPEED` / `MOTION_ANG_MAX_DEG` | `0.3` / `0.35` / `10` | discrete move/turn command speeds and turn-rate ceiling |
+| `MOTION_FRONT_STOP` | `0.30` | lidar obstacle distance (m) that halts forward motion |
+| `MOTION_TURN_SIGN` | `1` | set to `-1` if the robot turns opposite to the commanded direction |
+| `VLA_YOLO_WEIGHTS` / `VLA_YOLO_MIN_CONF` / `VLA_YOLO_IMGSZ` | `yolo11n.pt` / `0.3` / `960` | detector weights, confidence threshold, inference resolution |
+| `VLA_LOG_DIR` | `vla_logs` | per-step JSONL log directory |
+| `DEMO_MOCK` / `VLM_SKIP_MODEL` | — | run the UI without a robot / without loading the model |
+
+Full source of truth for these lives in [`vlm/webconsole/demo1/navloop.py`](vlm/webconsole/demo1/navloop.py)
+and [`vlm/webconsole/demo1/motion.py`](vlm/webconsole/demo1/motion.py).
+</details>
+
+### Brain 2 — NaVILA (VILA-8B)
+
+[NaVILA](https://github.com/AnjieCheng/NaVILA) is an 8B vision-language-action
+model trained specifically for language-guided navigation. It's integrated as a
+**System-2 / System-1 split brain**:
+
+```
+camera → 8-frame sliding buffer → HTTP → NaVILA (VILA-8B, System-2, GPU)
+      → mid-level action text → parser → MotionCmd
+      → MotionController → twist_mux → go2_driver → Go2 built-in gait (System-1)
+```
+
+- **System-2** (`navila_server.py`, its own conda env, port `8100`) loads the
+  8B checkpoint once and turns 8 camera frames + the instruction into an
+  action sentence (`"The next action is move forward 25 cm."`) — a pure
+  action policy, no chain-of-thought.
+- **System-1** is the same odom/lidar-closed-loop `MotionController` used
+  above; NaVILA never touches locomotion or gait, only high-level intent.
+
+First-time setup (downloads the code repo + checkpoint, patches `transformers`
+for Blackwell GPUs):
+```shell
+bash navila/setup_navila_blackwell.sh
+```
+
+One-command run (driver + NaVILA server + web console):
+```shell
+cd ros2_ws/src/navila
+./scripts/run.sh
+```
+Open `http://localhost:8001` and type an instruction **in English** (NaVILA is
+trained on English R2R-style instructions), e.g. *"Walk forward down the
+hallway and stop near the chair."* The reasoning panel shows the raw NaVILA
+output, per-token confidence, and a link to the exact 8 frames the model
+received.
+
+<details>
+<summary>Configuration reference — NaVILA brain</summary>
+
+| Env / arg | Default | Meaning |
+|---|---|---|
+| `VLA_BRAIN=navila` | — | routes control to the NaVILA server |
+| `VLA_NAVILA_URL` | `http://127.0.0.1:8100` | NaVILA server address |
+| `VLA_NAVILA_FRAMES` | `8` | frames sent per request (must match server `--num-frames`) |
+| `VLA_NAVILA_HISTORY_MAX` | `64` | raw history frames kept per instruction; uniformly resampled down to `VLA_NAVILA_FRAMES`, always including the first and current frame (matches the paper's sampling scheme) |
+| `NAVILA_FALLBACK` | `stop` | behavior when the model output fails to parse: `stop` \| `scan` |
+| `NAVILA_PREPROC` | `pad` | frame preprocessing: `fov` (undistort + forward crop, closest to training distribution) \| `pad` (full fisheye) \| `resize` \| `crop` |
+| server `--model-path` | — | path to the downloaded checkpoint |
+| server `--port` | `8100` | HTTP port |
+| server `--num-frames` | `8` | frames processed per request |
+
+Full parameter tables, the three Blackwell inference fixes (8-bit + eager
+attention + 4D causal mask — see [`navila/setup_navila_blackwell.sh`](navila/setup_navila_blackwell.sh)),
+and eval/debug tooling (`navila/tools/`) are documented inline in that script
+and in [`navila/scripts/`](navila/scripts).
+</details>
+
+## Acknowledgements & citations
+
+- Built on [abizovnuralem/go2_ros2_sdk](https://github.com/abizovnuralem/go2_ros2_sdk)
+  (RoboVerse community) — see [`LICENSE`](LICENSE).
+- **NaVILA** — An-Chieh Cheng et al., *"NaVILA: Legged Robot Vision-Language-Action
+  Model for Navigation"*, 2024. Code: [AnjieCheng/NaVILA](https://github.com/AnjieCheng/NaVILA),
+  checkpoint: `a8cheng/navila-llama3-8b-8f` on Hugging Face. See the official
+  repository for the paper and up-to-date citation.
+- **Qwen2.5-VL** — Alibaba Qwen team, model `Qwen/Qwen2.5-VL-3B-Instruct`.
+- **Ultralytics YOLO11** — [ultralytics/ultralytics](https://github.com/ultralytics/ultralytics).
+- **Nav2** and **slam_toolbox** — the ROS 2 Navigation Working Group /
+  [ros-navigation](https://github.com/ros-navigation) and
+  [SteveMacenski/slam_toolbox](https://github.com/SteveMacenski/slam_toolbox).
+- **Point Cloud Library (PCL)** — used for the SOR/ROR filtering pipeline in `go2_navigation`.
+- **twist_mux** — [ros-teleop/twist_mux](https://github.com/ros-teleop/twist_mux),
+  used to arbitrate teleop/VLA vs. Nav2 velocity commands.
+- **aiortc** — vendored WebRTC implementation used by the Go2 driver.
+
+## License
+
+BSD 2-Clause — see [`LICENSE`](LICENSE). Note: some vendored/derived source
+files under `go2_navigation/` carry a BSD-3-Clause header inherited from
+upstream; if you plan to redistribute, reconcile this with project counsel
+before publishing rather than assuming one license governs the whole tree.

docker/Dockerfile

@@ -0,0 +1,63 @@
+# Go2 ROS 2 workspace image: driver + navigation (Nav2/SLAM) + local VLA stack (vlm).
+#
+# Targets the same distro this project is developed/tested against (see README).
+# GPU-backed inference (local Qwen2.5-VL brain, YOLO) needs the NVIDIA Container
+# Toolkit on the host and a GPU reservation at `docker run`/`docker compose` time —
+# no CUDA base image is required since the pip wheels ship their own CUDA runtime.
+#
+# NaVILA is intentionally NOT part of this image: it needs its own conda env,
+# hand-patched transformers build, and a ~16GB checkpoint pulled at setup time —
+# see navila/setup_navila_blackwell.sh and run it on bare metal.
+FROM ros:jazzy-ros-base
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+# --- System packages (stable layer, only invalidated by this list changing) ---
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        curl \
+        gnupg2 \
+        lsb-release \
+        software-properties-common \
+        python3-pip \
+        clang \
+        portaudio19-dev \
+        mesa-utils \
+        libgl1-mesa-dev \
+        ffmpeg \
+        ros-${ROS_DISTRO}-rviz2 \
+        ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \
+        ros-${ROS_DISTRO}-image-tools \
+        ros-${ROS_DISTRO}-vision-msgs \
+    && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /ros2_ws
+
+# --- Python deps in their own layer: only rebuilt when requirements.txt changes ---
+COPY requirements.txt src/requirements.txt
+# Ubuntu 24.04 marks the system interpreter "externally managed" (PEP 668); this
+# image is disposable so overriding that guard is the pragmatic choice here.
+RUN pip install --no-cache-dir --break-system-packages 'numpy<2.0.0' && \
+    pip install --no-cache-dir --break-system-packages -r src/requirements.txt
+
+# --- Workspace source + ROS deps + build ---
+COPY . src/
+RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \
+    rosdep update && \
+    rosdep install --from-paths src --ignore-src -r -y && \
+    colcon build --symlink-install
+
+RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> /root/.bashrc && \
+    echo "source /ros2_ws/install/setup.bash" >> /root/.bashrc && \
+    sed --in-place --expression \
+        '$isource "/ros2_ws/install/setup.bash"' \
+        /ros_entrypoint.sh
+
+# Connection defaults — override at `docker run -e` / compose `environment:`,
+# never bake real robot IPs or tokens into the image.
+ENV ROBOT_IP=""
+ENV ROBOT_TOKEN=""
+ENV CONN_TYPE="webrtc"
+ENV WEBRTC_SERVER_HOST="0.0.0.0"
+ENV WEBRTC_SERVER_PORT="9991"
+
+CMD ["ros2", "launch", "go2_navigation", "bringup.launch.py"]

docker/docker-compose.yml

@@ -0,0 +1,37 @@
+services:
+  # Driver + navigation (Nav2/SLAM). Default profile — `docker compose up go2`.
+  go2:
+    build:
+      context: ..
+      dockerfile: docker/Dockerfile
+    image: go2-nav-vla:latest
+    environment:
+      - ROBOT_IP=${ROBOT_IP:?set ROBOT_IP to your Go2's LAN address}
+      - ROBOT_TOKEN=${ROBOT_TOKEN:-}
+      - CONN_TYPE=${CONN_TYPE:-webrtc}
+      - DISPLAY=${DISPLAY:-}
+    volumes:
+      - /tmp/.X11-unix:/tmp/.X11-unix:ro   # X11 forwarding, e.g. for rviz2
+      - ${HOME}/.Xauthority:/root/.Xauthority:rw
+    network_mode: host   # required: ROS 2 DDS discovery + Go2 WebRTC live on the LAN
+    privileged: true      # required: joystick/input device access
+    devices:
+      - /dev/input:/dev/input
+    restart: unless-stopped
+    command: ["ros2", "launch", "go2_navigation", "bringup.launch.py"]
+
+  # Local VLA brain (Qwen2.5-VL) + web console, port 8001. Opt-in:
+  #   docker compose --profile vla up vlm
+  # Requires the NVIDIA Container Toolkit on the host for GPU access.
+  vlm:
+    extends: go2
+    profiles: ["vla"]
+    working_dir: /ros2_ws/src
+    deploy:
+      resources:
+        reservations:
+          devices:
+            - driver: nvidia
+              count: all
+              capabilities: [gpu]
+    command: ["./vlm/scripts/run_demo1.sh"]

docker/entrypoint.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+set -e
+
+# Source ROS2 environment
+source /opt/ros/${ROS_DISTRO}/setup.bash
+source /ros2_ws/install/setup.bash
+
+# Execute the command passed to docker run
+exec "$@" 

go2_control/go2_control/keyboard_teleop.py

@@ -0,0 +1,150 @@
+"""Điều khiển Go2 bằng bàn phím (giữ phím = đi, buông = dừng).
+
+Bắn TwistStamped vào /cmd_vel_joy (qua twist_mux, priority cao -> đè Nav2).
+
+Phím (KHÔNG cần Enter):
+    ↑ / ↓     tiến / lùi        (vx)
+    ← / →     đi ngang trái/phải (vy)  — Go2 hỗ trợ
+    q / e     xoay trái / phải  (yaw)
+    space     dừng ngay
+    x / Ctrl-C thoát
+
+Cơ chế: terminal ở chế độ raw nên nhận phím tức thời. GIỮ phím -> hệ điều hành
+tự lặp phím liên tục -> vận tốc được giữ. BUÔNG phím -> hết lặp -> sau
+`hold_timeout` giây robot tự dừng (dead-man). Bấm phím ngược chiều -> đảo vận
+tốc ngay (hãm/lùi lại). Tốc độ chỉnh qua ROS param.
+"""
+import os
+import sys
+import select
+import termios
+import tty
+import time
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TwistStamped
+
+HELP = __doc__
+
+
+class KeyboardTeleop(Node):
+    def __init__(self):
+        super().__init__("go2_keyboard_teleop")
+
+        self.declare_parameter("cmd_topic", "/cmd_vel_joy")
+        self.declare_parameter("frame_id", "base_link")
+        self.declare_parameter("lin_speed", 0.4)      # m/s tiến/lùi
+        self.declare_parameter("lat_speed", 0.3)      # m/s đi ngang
+        self.declare_parameter("ang_speed", 0.8)      # rad/s xoay
+        self.declare_parameter("hold_timeout", 0.3)   # s không thấy phím -> dừng
+        self.declare_parameter("publish_rate", 50.0)  # Hz
+
+        gp = self.get_parameter
+        self.topic = gp("cmd_topic").get_parameter_value().string_value
+        self.frame_id = gp("frame_id").get_parameter_value().string_value
+        self.lin = gp("lin_speed").get_parameter_value().double_value
+        self.lat = gp("lat_speed").get_parameter_value().double_value
+        self.ang = gp("ang_speed").get_parameter_value().double_value
+        self.hold = gp("hold_timeout").get_parameter_value().double_value
+        self.dt = 1.0 / max(gp("publish_rate").get_parameter_value().double_value, 1.0)
+
+        self.pub = self.create_publisher(TwistStamped, self.topic, 10)
+        self._vx = self._vy = self._wz = 0.0
+        self._last_key = 0.0
+
+        self.get_logger().info(
+            f"Keyboard teleop -> {self.topic} "
+            f"(lin={self.lin} lat={self.lat} ang={self.ang}, dead-man={self.hold}s)")
+
+    def _apply(self, vx, vy, wz):
+        self._vx, self._vy, self._wz = vx, vy, wz
+        self._last_key = time.monotonic()
+
+    def _handle(self, keys):
+        """keys: chuỗi byte đã đọc trong tick này (đã drain hết)."""
+        i, n = 0, len(keys)
+        while i < n:
+            c = keys[i]
+            if c == "\x1b" and i + 2 < n + 1 and keys[i + 1:i + 2] == "[":
+                seq = keys[i + 2:i + 3]
+                if seq == "A":
+                    self._apply(self.lin, 0.0, 0.0)
+                elif seq == "B":
+                    self._apply(-self.lin, 0.0, 0.0)
+                elif seq == "D":
+                    self._apply(0.0, self.lat, 0.0)
+                elif seq == "C":
+                    self._apply(0.0, -self.lat, 0.0)
+                i += 3
+                continue
+            if c in ("q", "Q"):
+                self._apply(0.0, 0.0, self.ang)
+            elif c in ("e", "E"):
+                self._apply(0.0, 0.0, -self.ang)
+            elif c == " ":
+                self._apply(0.0, 0.0, 0.0)
+            elif c in ("x", "X", "\x03"):
+                raise KeyboardInterrupt
+            i += 1
+
+    def _publish(self):
+        # dead-man: quá lâu không thấy phím -> dừng (buông phím = dừng)
+        if time.monotonic() - self._last_key > self.hold:
+            self._vx = self._vy = self._wz = 0.0
+        msg = TwistStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = self.frame_id
+        msg.twist.linear.x = self._vx
+        msg.twist.linear.y = self._vy
+        msg.twist.angular.z = self._wz
+        self.pub.publish(msg)
+
+    def run(self):
+        if not sys.stdin.isatty():
+            self.get_logger().error(
+                "stdin không phải terminal -> không đọc được phím. "
+                "Chạy trực tiếp `ros2 run go2_control keyboard_teleop` trong terminal.")
+            return
+        print(HELP)
+        fd = sys.stdin.fileno()
+        old = termios.tcgetattr(fd)
+        try:
+            tty.setcbreak(fd)   # tức thời, không cần Enter
+            while rclpy.ok():
+                # drain mọi byte đang chờ trong tick này
+                buf = ""
+                while select.select([fd], [], [], 0)[0]:
+                    ch = os.read(fd, 1).decode(errors="ignore")
+                    if not ch:
+                        break
+                    buf += ch
+                if buf:
+                    self._handle(buf)
+                self._publish()
+                rclpy.spin_once(self, timeout_sec=0)
+                time.sleep(self.dt)
+        except KeyboardInterrupt:
+            pass
+        finally:
+            termios.tcsetattr(fd, termios.TCSADRAIN, old)
+            # gửi vài lệnh dừng cho chắc
+            for _ in range(3):
+                self._apply(0.0, 0.0, 0.0)
+                self._publish()
+                time.sleep(0.01)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = KeyboardTeleop()
+    try:
+        node.run()
+    finally:
+        if rclpy.ok():
+            node.destroy_node()
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

go2_control/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>go2_control</name>
+  <version>0.0.0</version>
+  <description>Keyboard teleop for Unitree Go2 (arrows = move, q/e = rotate) publishing TwistStamped to /cmd_vel_joy</description>
+  <maintainer email="dsclabs.contact@gmail.com">dsc-labs</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

go2_control/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/go2_control
+[install]
+install_scripts=$base/lib/go2_control

go2_control/setup.py

@@ -0,0 +1,25 @@
+from setuptools import find_packages, setup
+
+package_name = 'go2_control'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='dsc-labs',
+    maintainer_email='dsclabs.contact@gmail.com',
+    description='Keyboard teleop for Unitree Go2 (publishes TwistStamped to /cmd_vel_joy)',
+    license='BSD-3-Clause',
+    entry_points={
+        'console_scripts': [
+            'keyboard_teleop = go2_control.keyboard_teleop:main',
+        ],
+    },
+)

go2_interfaces/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.8)
+project(go2_interfaces)
+
+# Fix for CMake policy CMP0148 warning
+if(POLICY CMP0148)
+  cmake_policy(SET CMP0148 OLD)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  # Original go2_interfaces messages
+  "msg/Go2Cmd.msg"
+  "msg/Go2State.msg"
+  "msg/Go2Move.msg"
+  "msg/Go2RpyCmd.msg"
+  "msg/IMU.msg"
+  
+  # Merged from unitree_go
+  "msg/AudioData.msg"
+  "msg/BmsCmd.msg"
+  "msg/BmsState.msg"
+  "msg/Error.msg"
+  "msg/Go2FrontVideoData.msg"
+  "msg/HeightMap.msg"
+  "msg/InterfaceConfig.msg"
+  "msg/LidarState.msg"
+  "msg/LowCmd.msg"
+  "msg/LowState.msg"
+  "msg/MotorCmd.msg"
+  "msg/MotorCmds.msg"
+  "msg/MotorState.msg"
+  "msg/MotorStates.msg"
+  "msg/PathPoint.msg"
+  "msg/Req.msg"
+  "msg/Res.msg"
+  "msg/SportModeCmd.msg"
+  "msg/SportModeState.msg"
+  "msg/TimeSpec.msg"
+  "msg/UwbState.msg"
+  "msg/UwbSwitch.msg"
+  "msg/VoxelMapCompressed.msg"
+  "msg/VoxelHeightMapState.msg"
+  "msg/WebRtcReq.msg"
+  "msg/WirelessController.msg"
+
+  DEPENDENCIES geometry_msgs # Add packages that above messages depend on
+)
+
+
+ament_package()

go2_interfaces/msg/AudioData.msg

@@ -0,0 +1,2 @@
+uint64 time_frame
+uint8[] data

go2_interfaces/msg/BmsCmd.msg

@@ -0,0 +1,2 @@
+uint8 off
+uint8[3] reserve

go2_interfaces/msg/BmsState.msg

@@ -0,0 +1,9 @@
+uint8 version_high
+uint8 version_low
+uint8 status
+uint8 soc
+int32 current
+uint16 cycle
+int8[2] bq_ntc
+int8[2] mcu_ntc
+uint16[15] cell_vol

go2_interfaces/msg/Error.msg

@@ -0,0 +1,2 @@
+uint32 source
+uint32 state

go2_interfaces/msg/Go2Cmd.msg

@@ -0,0 +1 @@
+uint16 cmd

go2_interfaces/msg/Go2FrontVideoData.msg

@@ -0,0 +1,8 @@
+# Time frame as a 64-bit unsigned integer
+uint64 time_frame
+
+# Resolution as a 16-bit signed integer
+int16 resolution
+
+# Data as a sequence of bytes (octets)
+uint8[] data

go2_interfaces/msg/Go2Move.msg

@@ -0,0 +1,3 @@
+float32 x
+float32 y
+float32 z

go2_interfaces/msg/Go2RpyCmd.msg

@@ -0,0 +1,3 @@
+float32 roll
+float32 pitch
+float32 yaw

go2_interfaces/msg/Go2State.msg

@@ -0,0 +1,12 @@
+uint8 mode
+int32 progress
+uint8 gait_type	
+float32 foot_raise_height		  
+float32[3] position 
+float32 body_height			  
+float32[3] velocity 			   
+float32[4] range_obstacle
+int16[4] foot_force
+float32[12] foot_position_body
+float32[12] foot_speed_body
+

go2_interfaces/msg/HeightMap.msg

@@ -0,0 +1,15 @@
+# Header
+float64 stamp         # timestamp
+string frame_id      # world frame id
+
+# Map info
+float32 resolution     # The map resolution [m/cell]
+uint32 width  # Map width along x-axis [cells]
+uint32 height # Map height alonge y-axis [cells]
+float32[2] origin      # Map frame origin xy-position [m], the xyz-axis direction of map frame is aligned with the world frame
+
+# Map data, in x-major order, starting with [0,0], ending with [width, height]
+# For a cell whose 2d-array-index is [ix, iy]，
+#    its position in world frame is: [ix * resolution + origin[0], iy * resolution + origin[1]]
+#    its cell value is: data[width * iy + ix]
+float32[] data

go2_interfaces/msg/IMU.msg

@@ -0,0 +1,5 @@
+float32[4] quaternion
+float32[3] gyroscope
+float32[3] accelerometer
+float32[3] rpy
+int8 temperature

go2_interfaces/msg/InterfaceConfig.msg

@@ -0,0 +1,3 @@
+uint8 mode
+uint8 value
+uint8[2] reserve

go2_interfaces/msg/LidarState.msg

@@ -0,0 +1,17 @@
+float64 stamp
+string firmware_version
+string software_version
+string sdk_version
+float32 sys_rotation_speed
+float32 com_rotation_speed
+uint8 error_state
+float32 cloud_frequency
+float32 cloud_packet_loss_rate
+uint32 cloud_size
+uint32 cloud_scan_num
+float32 imu_frequency
+float32 imu_packet_loss_rate
+float32[3] imu_rpy
+float64 serial_recv_stamp
+uint32 serial_buffer_size
+uint32 serial_buffer_read

go2_interfaces/msg/LowCmd.msg

@@ -0,0 +1,15 @@
+uint8[2] head
+uint8 level_flag
+uint8 frame_reserve
+uint32[2] sn
+uint32[2] version
+uint16 bandwidth
+MotorCmd[20] motor_cmd
+BmsCmd bms_cmd
+uint8[40] wireless_remote
+uint8[12] led
+uint8[2] fan
+uint8 gpio
+uint32 reserve
+uint32 crc
+

go2_interfaces/msg/LowState.msg

@@ -0,0 +1,22 @@
+uint8[2] head
+uint8 level_flag
+uint8 frame_reserve
+uint32[2] sn
+uint32[2] version
+uint16 bandwidth
+IMU imu_state
+MotorState[20] motor_state
+BmsState bms_state
+int16[4] foot_force
+int16[4] foot_force_est
+uint32 tick
+uint8[40] wireless_remote
+uint8 bit_flag
+float32 adc_reel
+int8 temperature_ntc1
+int8 temperature_ntc2
+float32 power_v
+float32 power_a
+uint16[4] fan_frequency
+uint32 reserve
+uint32 crc

go2_interfaces/msg/MotorCmd.msg

@@ -0,0 +1,7 @@
+uint8 mode
+float32 q
+float32 dq
+float32 tau
+float32 kp
+float32 kd
+uint32[3] reserve

go2_interfaces/msg/MotorCmds.msg

@@ -0,0 +1 @@
+MotorCmd[] cmds

go2_interfaces/msg/MotorState.msg

@@ -0,0 +1,11 @@
+uint8 mode
+float32 q
+float32 dq
+float32 ddq
+float32 tau_est
+float32 q_raw
+float32 dq_raw
+float32 ddq_raw
+int8 temperature
+uint32 lost
+uint32[2] reserve

go2_interfaces/msg/MotorStates.msg

@@ -0,0 +1 @@
+MotorState[] states

go2_interfaces/msg/PathPoint.msg

@@ -0,0 +1,7 @@
+float32 t_from_start
+float32 x
+float32 y
+float32 yaw
+float32 vx
+float32 vy
+float32 vyaw

go2_interfaces/msg/Req.msg

@@ -0,0 +1,2 @@
+string uuid
+string body

go2_interfaces/msg/Res.msg

@@ -0,0 +1,3 @@
+string uuid
+uint8[] data
+string body

go2_interfaces/msg/SportModeCmd.msg

@@ -0,0 +1,13 @@
+uint8 mode
+uint8 gait_type
+uint8 speed_level
+float32 foot_raise_height
+float32 body_height
+float32[2] position
+float32[3] euler
+float32[2] velocity
+float32 yaw_speed
+BmsCmd bms_cmd
+PathPoint[30] path_point
+
+

go2_interfaces/msg/SportModeState.msg

@@ -0,0 +1,17 @@
+TimeSpec stamp
+uint32 error_code
+IMU imu_state
+uint8 mode
+float32 progress
+uint8 gait_type
+float32 foot_raise_height
+float32[3] position
+float32 body_height
+float32[3] velocity
+float32 yaw_speed
+float32[4] range_obstacle
+int16[4] foot_force
+float32[12] foot_position_body
+float32[12] foot_speed_body
+
+

go2_interfaces/msg/TimeSpec.msg

@@ -0,0 +1,5 @@
+# Time indicates a specific point in time, relative to a clock's 0 point.
+# The seconds component, valid over all int32 values.
+int32 sec
+# The nanoseconds component, valid in the range [0, 10e9).
+uint32 nanosec

go2_interfaces/msg/UwbState.msg

@@ -0,0 +1,19 @@
+uint8[2] version
+uint8 channel
+uint8 joy_mode
+float32 orientation_est
+float32 pitch_est
+float32 distance_est
+float32 yaw_est
+float32 tag_roll
+float32 tag_pitch
+float32 tag_yaw
+float32 base_roll
+float32 base_pitch
+float32 base_yaw
+float32[2] joystick
+uint8 error_state
+uint8 buttons
+uint8 enabled_from_app
+
+

go2_interfaces/msg/UwbSwitch.msg

@@ -0,0 +1 @@
+uint8 enabled

go2_interfaces/msg/VoxelHeightMapState.msg

@@ -0,0 +1,14 @@
+# Timestamp (in seconds since epoch)
+float64 stamp
+
+# Timestamp for the point cloud (in seconds)
+float64 stamp_cloud
+
+# Timestamp for odometry (in seconds)
+float64 stamp_odom
+
+# Size of the height map
+uint32 height_map_size
+
+# Size of the voxel map
+uint32 voxel_map_size

go2_interfaces/msg/VoxelMapCompressed.msg

@@ -0,0 +1,21 @@
+# Timestamp (in seconds since epoch)
+float64 stamp
+
+# Frame ID for the coordinate frame
+string frame_id
+
+# Resolution of the voxel map
+float64 resolution
+
+# Origin of the voxel map (x, y, z)
+float64[3] origin
+
+# Width of the voxel map in each dimension (x, y, z)
+int16[3] width
+
+# Source size of the compressed data
+uint64 src_size
+
+# Compressed voxel data
+uint8[] data
+

go2_interfaces/msg/WebRtcReq.msg

@@ -0,0 +1,10 @@
+# message header id. if 0, it will be assigned automatically
+int64  id
+# topic name on dog's side including rt/ prefix
+string topic
+# api_id, see https://wiki.theroboverse.com/en/unitree-go2-app-console-commands#sending-commands-to-go2
+int64 api_id
+# payload for specific api_id
+string parameter
+# priority of the message. 0 non-priority, 1 priority
+uint8 priority

go2_interfaces/msg/WirelessController.msg

@@ -0,0 +1,5 @@
+float32 lx
+float32 ly
+float32 rx
+float32 ry
+uint16 keys