kassam/G1_Lootah
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
G1_Lootah/Lidar/README.md

11 KiB
Raw Permalink Blame History

G1 SLAM Stack

Production SLAM for Unitree G1 Edu humanoid + Livox MID-360 head-mounted LiDAR.

KISS-ICP scan matching, voxel persistence filtering, geometric place recognition, pose-graph loop closure, submap mapping with atomic checkpointing, and a Nav2-compatible map export pipeline. Runs as a multi-process worker with a PyQt commander UI.

Hardware

Item Spec
Robot Unitree G1 Edu (~1.32 m, ~0.30 m stride)
LiDAR Livox MID-360 (head-mounted, z ≈ 1.30 m)
LiDAR rate 10 Hz, ~200 K pts/s, 360° H-FoV, -7° to +52° V-FoV
LiDAR range 0.10 m – 40 m (10% reflectivity)
Compute Workstation (mapping/UI) + Jetson Orin NX (deploy target)

Requirements

System

  • Linux x86_64 (workstation) or aarch64 (Jetson Orin NX)
  • Python ≥ 3.10
  • A wired NIC (Gigabit) on the LiDAR network — typically enp3s0

Native libraries

  • Livox-SDK2 — C++ SDK from Livox. The repo ships with Livox-SDK2/ as a submodule. Build and install: 
    cd Livox-SDK2
mkdir -p build && cd build
cmake .. && make -j
sudo make install
    This places liblivox_lidar_sdk_* under /usr/local/lib/.

Python packages

pip install numpy scipy open3d kiss-icp PyQt6 pyqtgraph
Package Why
numpy Point-cloud math throughout
scipy KD-trees in cleanup / loop closure
open3d Point-cloud I/O (.ply read/write)
kiss-icp Scan-to-scan ICP backend
PyQt6 Commander GUI
pyqtgraph OpenGL viewport in the GUI

The Livox driver wrapper (livox2_python.py) uses ctypes against the installed liblivox_lidar_sdk_* — no separate Python binding required.

Network

LiDAR ships at 192.168.123.120. The workstation NIC must be on the same /24:

sudo ip addr add 192.168.123.222/24 dev enp3s0
sudo ip link set enp3s0 up
ping 192.168.123.120   # verify

UDP ports 56100–56501 must not be in use (close LivoxViewer 2 / other SLAM instances first).

How to run

1. Verify the LiDAR is online

ping 192.168.123.120

If no reply: check cable, NIC IP (ip addr show enp3s0), and that no other software is bound to the LiDAR's ports.

2. Sanity-check the config

Open SLAM_Config.json and mid360_config.json. Confirm:

  • network.default_host_ip matches your NIC IP
  • mid360_config.json host IPs match
  • extrinsic_parameter.z matches the actual LiDAR mount height

3. Launch the GUI

cd /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar
python3 SLAM_GUI.py

The SLAM Commander window opens.

4. Map a new place

  1. Click CONNECT — the worker spins up and connects to the LiDAR.
  2. Click MAP NEW PLACE — sets profile to MAP_NEW (long decay, fine voxels).
  3. Click START — mapping begins; points appear in the viewport.
  4. Walk the robot through the space.
  5. Click STOP — if SAVE ARMED is checked, the map saves to DataMap/<name>.ply.

Use AUTOSAVE for long sessions (saves every N seconds).

5. Localize / navigate in an existing map

  1. CONNECT.
  2. NAVIGATE IN MAPPED PLACE — sets profile to LOCALIZE_MAP.
  3. LOAD MAP — pick a .ply from DataMap/.
  4. START — the system runs ICP against the loaded map. The status bar shows TRACKING once aligned.
  5. Use the mission/waypoint controls to drive goals.

If the robot starts in an unknown spot inside the map, global relocalization runs automatically: session-memory cache → place recognition → brute-force anchor × yaw search.

6. Override the config path

SLAM_CONFIG=/path/to/custom.json python3 SLAM_GUI.py

7. Headless replay / regression

python3 SLAM_Replay.py --recording <path>.lvx2

File map

File Purpose
SLAM_GUI.py PyQt commander UI
SLAM_engine.py Config dataclasses, worker bootstrap, EngineConfig
SLAM_worker.py Main SLAM loop: ingest, ICP, filter, map, checkpoint, save, navigate
SLAM_LocalizationService.py odom/map/ref frame transforms
SLAM_LoopClosure.py Keyframe loop closure with SE3-slerp error distribution
SLAM_PlaceRecognition.py Geometric anchor descriptors for global relocalization
SLAM_StateMachine.py TRACKING / DEGRADED / LOST / RECOVERY
SLAM_Submap.py Local + global submap mapper, atomic .pkl checkpointing
SLAM_Filter.py Voxel persistence filter + indoor map quality filter
SLAM_MAP.py Stable-map layer, .ply export via Open3D
SLAM_Navigation.py Nav2-compatible YAML/PGM export, A* planner
SLAM_NavRuntime.py Live cost-map for runtime planning
SLAM_Mission.py Waypoint missions
SLAM_Fusion.py LiDAR + IMU + (wheel/vision) pose fusion
SLAM_Safety.py E-stop / stale-localization watchdog
SLAM_Session.py Session memory (cached slam→ref alignments per map)
SLAM_Replay.py Offline replay / regression harness
SLAM_Validation.py Startup self-check
SLAM_Diagnostics.py Crash logging, performance snapshots
SLAM_Transforms.py SE3 utilities, pose deltas, slerp
livox2_python.py Livox-SDK2 ctypes wrapper, with tag-byte noise filter
SLAM_Config.json Single source of truth for tuning
mid360_config.json Livox driver config (IPs, ports, extrinsics)

Configuration reference

Network — IPs & Interfaces

Source Setting Value
SLAM_Config.json network.default_interface enp3s0
SLAM_Config.json network.default_host_ip 192.168.123.222 (workstation)
mid360_config.json lidar_configs[0].ip 192.168.123.120 (LiDAR — Unitree default)
mid360_config.json host_net_info.*_ip 192.168.123.222 (matches workstation)

Reference (Unitree docs): default LiDAR IP 192.168.123.120, host on 192.168.123.x/24, gateway 192.168.123.1.

LiDAR UDP ports — mid360_config.json

Direction Channel LiDAR side Host side
Control cmd_data 56100 56101
Push messages push_msg 56200 56201
Point cloud point_data 56300 56301
IMU data imu_data 56400 56401
Logs log_data 56500 56501

LiDAR extrinsics — mid360_config.json (head mount, G1)

Param Value Note
roll 0.0°
pitch 0.0° head mount, level (Unitree default)
yaw 0.0°
x 0.0 m
y 0.0 m
z 1.30 m LiDAR atop head, ≈ floor + 1.30 m
pcl_data_type 1 Cartesian High
pattern_mode 0 non-repetitive

SLAM core tuning — SLAM_Config.json highlights

Section Key Value Purpose
slam slam_voxel_size 0.12 m ICP voxel
max_range 50.0 m LiDAR clip
filter voxel_size 0.20 m persistence base
hits_threshold 4 (overridden by profiles)
persistence.decay_seconds 3.0 s (overridden by profiles)
persistence.max_voxels 2,000,000 mem cap
map display_voxel / save_voxel 0.08 / 0.05 m GUI / .ply resolution
min_points_to_save 550 guard
map_quality near_min_range_m 0.15 m hardware min is 0.10
body_exclusion x[-0.20, 0.25] · y[±0.25] · z[-1.40, -0.10] head-mount G1 body box
map_cleanup keep_largest_n / period_sec 2 / 6.0 s islands removed every 6 s
submap_mapping local_voxel_m / global_voxel_m 0.08 / 0.14 m
checkpoint_interval_sec 60.0 s atomic .pkl save
navigation_export z_min_m / z_max_m 0.05 / 1.40 m floor+5 cm to 1.40 m
resolution_m 0.05 m Nav2 grid
state_machine min_good_to_recover 3 RECOVERY → TRACKING
mission waypoint_tolerance_m 0.55 m G1 ~0.30 m stride margin
safety stop_radius_m 0.50 m
stale_localization_sec 1.5 s
fusion enabled true LiDAR + IMU pose fusion
imu_weight / lidar_weight 0.25 / 1.0
runtime publish_hz 12.0 GUI/status update rate
livox tag_filter true Unitree-recommended noise drop
loop_closure enabled false optimizer is real, but disabled

Stability profiles — hardcoded in SLAM_worker.py:1957-1961

Profile hit_threshold decay_seconds voxel_size density
MAP_NEW 2 1800.0 0.10 MEDIUM
LOCALIZE_MAP 3 45.0 0.20 MEDIUM
LIVE_NAV_MAP 2 18.0 0.20 MEDIUM
LIVE_NAV_NO_MAP 2 14.0 0.18 HIGH
QUICK_DEMO 2 8.0 0.20 HIGH
BALANCED (from config) (from config) (from config) MEDIUM

File locations

G1_Lootah/Lidar/
├── SLAM_Config.json          # main config (single source of truth)
├── mid360_config.json        # Livox driver (read by SDK2)
└── DataMap/                  # maps + submap_checkpoint.pkl + session memory

Override path: SLAM_CONFIG=/path/to/other.json.

Workflows

Workflow Profile When to use
Map a new place MAP_NEW Building a fresh map. Long decay (30 min), 0.10 m voxels.
Navigate in mapped place LOCALIZE_MAP Localize against an existing .ply.
Live nav with map LIVE_NAV_MAP Navigation + slow map updates.
Live nav (no map) LIVE_NAV_NO_MAP Pure obstacle-avoidance, no persistent map.
Quick demo QUICK_DEMO Short-window mapping for demos.

The current profile shows in the GUI status bar.

Outputs

DataMap/
├── *.ply                       # saved point cloud maps
├── *.yaml + *.pgm              # Nav2-compatible map_server bundles
├── submap_checkpoint.pkl       # atomic submap snapshot (auto-restored on next start)
└── SLAM_session_memory.json    # cached relocalization alignments per map

Frame conventions

  • World z = 0 is floor.
  • LiDAR origin at sensor frame's z = 0; extrinsic places it at body-frame z ≈ 1.30 m.
  • Body exclusion box in sensor frame: z = -1.40 (floor) to z = -0.10 (just below LiDAR).
  • ICP yields T_world_lidar; localization service maintains odom_to_map and odom_to_ref separately.

Deploy

Edit on the workstation only. Push to Jetson via scp:

scp -r /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar/ \
    unitree@<jetson-ip>:~/G1_Lootah/

Do not edit on the Jetson directly.

Common issues

  • Connect fails / Address already in use — another Livox app holds ports 56101–56501. Close LivoxViewer 2 / other SLAM instances.
  • No map / empty viewport — verify ping 192.168.123.120 works, NIC IP matches default_host_ip, and the LiDAR LED is solid.
  • Map ignores furniture — head-mount + -7° lower FoV creates a blind cone underneath; close-floor area is unscannable < ~10 m. Move the robot closer or accept the limitation.
  • Stray points in saved map — cleanup pass runs every 6 s and once more on save; if islands persist, lower map_cleanup.keep_largest_n.
  • Loop closure off — set loop_closure.enabled: true in config; the optimizer (SLAM_LoopClosure._optimize) is fully wired.
  • SDKINIT failed — Livox-SDK2 not installed; rebuild Livox-SDK2/ and sudo make install.