Skip to content

Nav pt2: Alfred#2100

Open
jeff-hykin wants to merge 15 commits into
mainfrom
jeff/feat/flowbase
Open

Nav pt2: Alfred#2100
jeff-hykin wants to merge 15 commits into
mainfrom
jeff/feat/flowbase

Conversation

@jeff-hykin
Copy link
Copy Markdown
Member

@jeff-hykin jeff-hykin commented May 15, 2026

New robot (wheeled base) Alfred: for nav stack data collection

Testing:

dimos --rerun-host 0.0.0.0 run alfred-nav 

@jeff-hykin jeff-hykin requested a review from mustafab0 May 15, 2026 23:30
@greptile-apps
Copy link
Copy Markdown
Contributor

greptile-apps Bot commented May 15, 2026

Greptile Summary

This PR introduces Alfred, a new wheeled robot platform for nav-stack data collection, wiring together FastLio2 LiDAR, a simple nav stack, and a new Portal RPC-based high-level effector module. It also makes model_path optional in RobotConfig (with proper lazy-parse guards) and adds a bin/ci-check script for local/CI type-checking.

  • AlfredHighLevel: subscribes to cmd_vel, applies Alfred's inverted-Y-axis frame correction, and forwards holonomic velocity commands via Portal RPC using asyncio.to_thread to avoid blocking the event loop.
  • alfred_nav blueprint: connects FastLio2, nav stack, MovementManager, and AlfredHighLevel with explicit stream remappings.
  • RobotConfig: model_path is now optional, enabling wheel-only robots like Alfred that have no URDF.

Confidence Score: 4/5

Mostly safe to merge for data collection; one state-tracking bug in the auto-stop path warrants a fix before the module is used in agent-driven scenarios.

In _auto_stop_movement, _last_velocities is reset to zeros unconditionally because _send_velocity swallows its own exceptions and never re-raises — meaning a failed stop command leaves the robot moving while get_state() reports STOPPED.

dimos/robot/diy/alfred/effector_high_level.py — specifically the _auto_stop_movement method and its handling of _send_velocity return value.

Important Files Changed

Filename Overview
dimos/robot/diy/alfred/effector_high_level.py New high-level control module for Alfred via Portal RPC; _auto_stop_movement unconditionally resets _last_velocities to zeros even when the stop command fails, making get_state() report STOPPED when the robot may still be moving.
dimos/robot/diy/alfred/config.py New Alfred robot config; borrows G1 local-planner precomputed paths as a placeholder.
dimos/robot/diy/alfred/blueprints/alfred_nav.py New nav-stack blueprint wiring FastLio2, nav stack, MovementManager, and AlfredHighLevel with appropriate remappings; no issues found.
dimos/robot/config.py Makes model_path optional and adds clear ValueError guards; straightforward and safe.
bin/ci-check New CI script; uv pip install mypy sidesteps the lock file, risking version drift.
dimos/robot/all_blueprints.py Registers alfred-nav blueprint and alfred-high-level module.
dimos/robot/test_all_blueprints.py Adds alfred-nav to the self-hosted blueprint set; consistent with existing pattern.

Sequence Diagram

sequenceDiagram
    participant NS as Nav Stack
    participant AHL as AlfredHighLevel
    participant PT as Portal Client
    participant WD as Watchdog

    NS->>AHL: cmd_vel (Twist)
    AHL->>WD: cancel previous _stop_task
    AHL->>PT: asyncio.to_thread(set_target_velocity(vx, -vy, -wz))
    PT-->>AHL: OK / Exception
    alt send succeeded
        AHL->>WD: create_task(_auto_stop_movement(timeout))
        WD-->>PT: set_target_velocity(0,0,0) after timeout
    else send failed
        AHL-->>NS: return False
    end
Loading

Reviews (8): Last reviewed commit: "-" | Re-trigger Greptile

Comment thread dimos/robot/diy/alfred/effector_high_level.py Outdated
Comment thread dimos/robot/diy/alfred/effector_high_level.py
Comment on lines +33 to +35
# Mid-360 lidar: 0.20 m forward, 0.20 m right of base center, 0.10 m above ground.
# it is mounted at an angle but the livox driver will handle that automatically
"mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))),
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 The inline comment says the LiDAR is mounted "0.10 m above ground" but the z component passed to Pose is 0.30. The comment and the value are inconsistent — either the mount height or the comment needs to be corrected.

Suggested change
# Mid-360 lidar: 0.20 m forward, 0.20 m right of base center, 0.10 m above ground.
# it is mounted at an angle but the livox driver will handle that automatically
"mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))),
# Mid-360 lidar: 0.20 m forward, 0.20 m right of base center, 0.30 m above ground.
# it is mounted at an angle but the livox driver will handle that automatically
"mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))),

@codecov
Copy link
Copy Markdown

codecov Bot commented May 15, 2026

Codecov Report

❌ Patch coverage is 46.66667% with 64 lines in your changes missing coverage. Please review.
✅ All tests successful. No failed tests found.

Files with missing lines Patch % Lines
dimos/robot/diy/alfred/effector_high_level.py 35.48% 60 Missing ⚠️
dimos/robot/config.py 20.00% 3 Missing and 1 partial ⚠️

📢 Thoughts on this report? Let us know!

mustafab0
mustafab0 previously approved these changes May 15, 2026
Comment thread dimos/robot/diy/alfred/config.py
Comment thread dimos/robot/diy/alfred/effector_high_level.py Outdated
Comment thread dimos/robot/diy/alfred/effector_high_level.py
mustafab0
mustafab0 previously approved these changes May 16, 2026
Comment thread bin/ci-check Outdated
@jeff-hykin jeff-hykin closed this May 16, 2026
auto-merge was automatically disabled May 16, 2026 06:29

Pull request was closed

@jeff-hykin jeff-hykin reopened this May 16, 2026
@jeff-hykin jeff-hykin closed this May 16, 2026
@jeff-hykin jeff-hykin reopened this May 16, 2026
@jeff-hykin jeff-hykin closed this May 16, 2026
@jeff-hykin jeff-hykin reopened this May 16, 2026
Comment on lines +118 to +127
async def _auto_stop_movement(self, delay: float) -> None:
try:
await asyncio.sleep(delay)
except asyncio.CancelledError:
return
try:
await self._send_velocity(0.0, 0.0, 0.0)
self._last_velocities = [0.0, 0.0, 0.0]
except Exception as e:
logger.error(f"Auto-stop failed: {e}")
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P1 Auto-stop failure silently poisons _last_velocities state

_send_velocity catches all exceptions internally and always returns False on failure — it never re-raises. This means the except Exception branch in _auto_stop_movement is dead code, and self._last_velocities = [0.0, 0.0, 0.0] always executes even when the stop command failed. If the Portal link is down when the watchdog fires, the robot continues moving at the previous velocity while get_state() reports "STOPPED". Any agent or orchestrator polling get_state() to confirm the platform has halted will get incorrect information. Fix by checking the return value of _send_velocity before resetting _last_velocities.

@jeff-hykin jeff-hykin changed the title Alfred Nav pt2: Alfred May 16, 2026
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants