Skip to content

go2 motion: local planner on the raycaster's fine layer, at its pitch - #4237

Draft
leshy wants to merge 17 commits into
feat/ivan/go2_ddsfrom
feat/ivan/fine_local_map
Draft

leshy wants to merge 17 commits into
feat/ivan/go2_ddsfrom
feat/ivan/fine_local_map

Conversation

@leshy

@leshy leshy commented Sep 19, 2026

Copy link
Copy Markdown
Member

Stacked on #4235 (feat/ivan/go2_dds), which sits on #4203.

The motion stacks (go2-zenoh-motion, go2-zenoh-motion-pointlio, go2-dds-motion-pointlio) turn on the raycaster's fine layer and plan on it at its own pitch.

Raycaster: fine_divisor=2 (0.04 m cells), emit_fine=True publishes local_map_fine on the local-map cadence; LocalPlannerNative.local_map is remapped onto it. The MLS planner and the viewer keep the 0.08 m local_map.

Local planner: one setting, pointcloud_resolution (the map's voxel, default 0.08), replaces the compile-time pitches in both twins. Everything scales off it the way the old constants related: fine field = voxel / 2, lattice cell = 3 fine, period = 2 cells = 3 voxels, pad = 3 periods, snap = fine diagonal (Geom::of in planner.rs, pitches() in se2.py). At 0.08 the numbers are the old ones exactly; the pyo3 plan() and the native config take it too. The fine stacks pass voxel_size / 2, so they search a 0.06 m lattice on a 0.02 m field.

Verified: crate 59 + 5 tests and clippy -D warnings; both twins plan around the ring fixture at 0.08 and 0.04 and land on the goal; mypy + pytest-fast green. Not yet run on the robot: replan time at 0.04 is 4x the lattice nodes, worth a look on the Jetson.

🤖 Generated with Claude Code

GO2Zenoh's ports, command verbs, mount tree and camera intrinsics move to
Go2Base so a second robot-side connection (DDS) shares them. GO2Zenoh keeps
its Point-LIO extras: pointlio_map, the odom -> mid360_link edge, the mount
tree rooted at mid360_link and the L1 off at start.
Lifted out of go2web's dimos-helper: the CycloneDDS runtime and Go2 types,
the sport verb table, and the videohub RTP -> H.264 annex-B reassembly.
The module terminates cmd_vel as sport Move (clamped, dead-man StopMove)
and command verbs as sport requests, and publishes rt/utlidar/robot_odom
with its tf edge, the deskewed L1 cloud, and the front camera.

cyclonedds-sys is vendored from our fork with bindgen bumped: 0.55 cannot
parse the anonymous unions in CycloneDDS's headers with the libclang the
dev shell ships.
Same profile as GO2Zenoh (both are Go2Base), with the rust module as the
robot side. The config parity test pins what crosses to the rust struct.
Build goes through a tiny crate flake (nix/, so the path: snapshot never
carries target/) with nixpkgs' bindgenHook; the root dev shell change and
build.sh go. The binary carries an rpath to libddsc so it runs outside the
shell.

video_encoding=jpeg polls the videohub GetImageSample at video_fps onto a
new image port. rt/lowstate becomes joint_state, imu and battery (decimated
to lowstate_hz, battery at 1 Hz), rt/wirelesscontroller becomes joy, and
lidar_raw_on adds the undeskewed cloud plus the L1's own IMU. cmd_vel is no
longer clamped: limits live in the planner and follower.

The command port grows a "<verb> <arg>" form: obstacle-avoidance, joystick,
rage, brightness, led, volume, next to the sport verbs and the L1 switch.
Go2Base exposes them as rpcs named after GO2Connection's; GO2Zenoh warns on
the ones the go2web bridge does not resolve.
The Point-LIO motion stack over DDS: GO2DDS's native process listens on 7447
(the Go2 forwards it to the Jetson, so the viewer still dials go22) and the
other processes dial it on loopback. tf_root on Go2Base picks which frame the
live odometry moves (GO2Zenoh's mid360_link override becomes its default),
odom_tf on GO2DDS drops its own odom -> base_link edge when Point-LIO owns
odom.
The robot has no GitHub access, so the private fork comes along like
cyclonedds-sys already did. odom_topic and lidar_topic never vary on a Go2
and go; odom_tf folds into tf_root, which now crosses to rust.
The blueprint config layer re-validates every module kwarg from its fields,
and the field-less SessionConfig base rejected a pinned router's mode, listen
and connect as extras. The ping-pong example's --topology never parsed under
`dimos run`; now it does.
The native process refuses a topic it never declared, so a port the python
wrapper owns (GO2DDS's camera_info intrinsics) is now named in the config and
kept out of the topics handed over.
Point-LIO reads imu from the MID-360; GO2DDS's body IMU on the same topic
interleaved a second device into it. Remapped to body_imu.
emit_fine at fine_divisor=2, the planner's own field pitch (FINE = VOXEL / 2
in local_planner/search/se2.py); local_map_fine feeds LocalPlannerNative,
the MLS planner and the viewer keep the 0.08 m local_map.
One setting, the local map's voxel: the fine field is half of it, the lattice
cell three fine samples, the period two cells, the pad three periods, the
snap a fine diagonal. The stock 0.08 reproduces the old constants exactly;
both twins, the pyo3 binding and the native config take it. The fine motion
stacks plan at 0.04 on local_map_fine.
@leshy leshy changed the title go2 motion: local planner on the raycaster's fine layer go2 motion: local planner on the raycaster's fine layer, at its pitch Sep 19, 2026
The Command Center on 7779 came along even with visualization off; a
robot-side stack nobody browses now serves nothing.
The fitted sweep's 0° and 180° rows carried 0.416; the feet never leave the
trunk walking straight. Drift rows keep the fitted widths.
The 5 cm floor plus the 0.33 body refused a measured 51 cm passage on the
accumulated map. test_governor_creeps_in_tight_room still assumes the old
floor; untouched pending the real-world run.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant