Nav pt2: Alfred

.github/workflows/ci.yml

@@ -142,7 +142,7 @@ jobs:
       OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }}
       ANTHROPIC_API_KEY: ${{ secrets.ANTHROPIC_API_KEY }}
       ALIBABA_API_KEY: ${{ secrets.ALIBABA_API_KEY }}
-    timeout-minutes: 30
+    timeout-minutes: 45
     strategy:
       fail-fast: false
       matrix:

dimos/navigation/nav_stack/main.py

@@ -197,6 +197,7 @@ def nav_stack_rerun_config(
     agentic_debug: bool = False,
     show_registered_scan: bool = False,
     vis_throttle: float = 1.0,
+    default_max_hz: int = 60,
 ) -> dict[str, Any]:
     """Return a rerun config dict with nav stack visualization defaults.
 
@@ -206,10 +207,6 @@ def nav_stack_rerun_config(
     Use ``vis_throttle`` (make smaller) if there is crashing related to Rerun/Dimos-Viewer.
     """
     resolved = dict(user_config or {})
-    if vis_throttle != 1.0 and "max_hz" in resolved:
-        resolved["max_hz"] = {
-            entity: hz * vis_throttle for entity, hz in resolved["max_hz"].items()
-        }
     resolved.setdefault("blueprint", _default_rerun_blueprint)
     resolved.setdefault("pubsubs", [LCM()])
     resolved.setdefault("visual_override", {})
@@ -251,6 +248,13 @@ def nav_stack_rerun_config(
     static_entries = dict(resolved["static"])
     static_entries.setdefault("world/floor", _static_floor)
     resolved["static"] = static_entries
+    # scale/limit rendering (mostly preventing rerun from crashing)
+    resolved.setdefault("max_hz", {})
+    resolved["max_hz"] = {
+        each_entity: resolved["max_hz"].get(each_entity, default_max_hz) * vis_throttle
+        for each_entity in set(visual_override) | set(resolved["max_hz"])
+    }
+
     return resolved
 
 

dimos/robot/all_blueprints.py

@@ -16,6 +16,7 @@
 # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate.
 
 all_blueprints = {
+    "alfred-nav": "dimos.robot.diy.alfred.blueprints.alfred_nav:alfred_nav",
     "coordinator-basic": "dimos.control.blueprints.basic:coordinator_basic",
     "coordinator-cartesian-ik-mock": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_mock",
     "coordinator-cartesian-ik-piper": "dimos.control.blueprints.teleop:coordinator_cartesian_ik_piper",
@@ -118,6 +119,7 @@
 
 
 all_modules = {
+    "alfred-high-level": "dimos.robot.diy.alfred.effector_high_level.AlfredHighLevel",
     "arm-teleop-module": "dimos.teleop.quest.quest_extensions.ArmTeleopModule",
     "b-box-navigation-module": "dimos.navigation.bbox_navigation.BBoxNavigationModule",
     "b1-connection-module": "dimos.robot.unitree.b1.connection.B1ConnectionModule",

dimos/robot/config.py

@@ -53,7 +53,7 @@ class RobotConfig(BaseModel):
 
     # Required fields
     name: str
-    model_path: Path
+    model_path: Path | None = None
     end_effector_link: str | None = None
 
     # Physical dimensions (meters)
@@ -115,6 +115,11 @@ def _ensure_prefix(self) -> None:
     def _ensure_parsed(self) -> ModelDescription:
         """Parse model lazily on first access."""
         if self._parsed is None:
+            if self.model_path is None:
+                raise ValueError(
+                    f"RobotConfig '{self.name}' has no model_path — "
+                    "joint/link info is unavailable. Set model_path to a URDF/MJCF."
+                )
             self._parsed = parse_model(self.model_path, self.package_paths, self.xacro_args)
             self._ensure_prefix()
             if self.joint_names is None:
@@ -193,6 +198,11 @@ def to_robot_model_config(self) -> RobotModelConfig:
                 f"RobotConfig '{self.name}' has no end_effector_link — "
                 "cannot generate RobotModelConfig for manipulation."
             )
+        if self.model_path is None:
+            raise ValueError(
+                f"RobotConfig '{self.name}' has no model_path — "
+                "cannot generate RobotModelConfig for manipulation."
+            )
         bp = self.base_pose
         base_pose = PoseStamped(
             position=Vector3(x=bp[0], y=bp[1], z=bp[2]),

dimos/robot/diy/alfred/blueprints/alfred_nav.py

@@ -0,0 +1,82 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+import os
+from typing import Any
+
+from dimos.core.coordination.blueprints import autoconnect
+from dimos.core.global_config import global_config
+from dimos.hardware.sensors.lidar.fastlio2.module import FastLio2
+from dimos.navigation.movement_manager.movement_manager import MovementManager
+from dimos.navigation.nav_stack.main import create_nav_stack, nav_stack_rerun_config
+from dimos.robot.diy.alfred.config import ALFRED, LOCAL_PLANNER_PRECOMPUTED_PATHS
+from dimos.robot.diy.alfred.effector_high_level import AlfredHighLevel
+from dimos.visualization.vis_module import vis_module
+
+nav_config: dict[str, Any] = dict(
+    planner="simple",
+    vehicle_height=0.5,
+    max_speed=0.8,
+    terrain_analysis={
+        "obstacle_height_threshold": 0.15,
+        "ground_height_threshold": 0.10,
+        "sensor_range": 20,
+    },
+    local_planner={
+        "paths_dir": str(LOCAL_PLANNER_PRECOMPUTED_PATHS),
+        "publish_free_paths": False,
+    },
+    simple_planner={
+        "cell_size": 0.2,
+        "obstacle_height_threshold": 0.15,
+        "inflation_radius": 0.3,
+        "lookahead_distance": 2.0,
+        "replan_rate": 5.0,
+        "replan_cooldown": 2.0,
+    },
+)
+
+alfred_nav = (
+    autoconnect(
+        FastLio2.blueprint(
+            host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"),
+            lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"),
+            mount=ALFRED.internal_odom_offsets["mid360_link"],
+            map_freq=1.0,
+            config="default.yaml",
+        ),
+        create_nav_stack(**nav_config),
+        MovementManager.blueprint(),
+        AlfredHighLevel.blueprint(),
+        vis_module(
+            global_config.viewer,
+            rerun_config={
+                **nav_stack_rerun_config({"memory_limit": "1GB"}, vis_throttle=0.5),
+                "rerun_open": "native",
+            },
+        ),
+    )
+    .remappings(
+        [
+            # nav stack needs "registered_scan"
+            (FastLio2, "lidar", "registered_scan"),
+            (FastLio2, "global_map", "global_map_fastlio"),
+            # SimplePlanner / FarPlanner owns way_point — disconnect MovementManager's
+            (MovementManager, "way_point", "_mgr_way_point_unused"),
+        ]
+    )
+    .global_config(n_workers=8)
+)

dimos/robot/diy/alfred/config.py

@@ -0,0 +1,36 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.Quaternion import Quaternion
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.robot.config import RobotConfig
+from dimos.robot.unitree.g1.config import G1_LOCAL_PLANNER_PRECOMPUTED_PATHS
+
+DEFAULT_ADDRESS = "172.6.2.20:11323"
+
+# just as a starting point. May re-compute these later. In principle robot-specific
+LOCAL_PLANNER_PRECOMPUTED_PATHS = G1_LOCAL_PLANNER_PRECOMPUTED_PATHS
+
+ALFRED = RobotConfig(
+    name="alfred",
+    height_clearance=2.0,  # meters
+    width_clearance=1.0,
+    internal_odom_offsets={
+        # Mid-360 lidar: a bit forward, and a bit to the right of base center, above ground.
+        "mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))),
+    },
+)

dimos/robot/diy/alfred/effector_high_level.py

@@ -0,0 +1,162 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Alfred high-level control via Portal RPC.
+
+Subscribes to ``cmd_vel`` and forwards each Twist to the Alfred controller
+as a holonomic target velocity. The controller performs the wheel-level
+kinematics on-board, so this module hands off ``(vx, vy, wz)`` rather than
+computing per-wheel speeds locally.
+
+Frame convention: Alfred uses an inverted Y-axis vs. ROS, so ``vy`` and
+``wz`` are negated before being sent to the hardware.
+
+  Standard (ROS):     Alfred:
+      +Y                -Y
+      ↑                  ↑
+   ───┼──→ +X         ───┼──→ +X
+      |                  |
+"""
+
+from __future__ import annotations
+
+import asyncio
+from collections.abc import AsyncGenerator
+from typing import Any
+
+import numpy as np
+import portal
+
+from dimos.agents.annotation import skill
+from dimos.core.core import rpc
+from dimos.core.module import Module, ModuleConfig
+from dimos.core.stream import In
+from dimos.msgs.geometry_msgs.Twist import Twist
+from dimos.msgs.geometry_msgs.Vector3 import Vector3
+from dimos.robot.diy.alfred.config import DEFAULT_ADDRESS
+from dimos.utils.logging_config import setup_logger
+
+logger = setup_logger()
+
+
+class AlfredHighLevelConfig(ModuleConfig):
+    address: str = DEFAULT_ADDRESS
+    cmd_vel_timeout: float = 0.2
+
+
+class AlfredHighLevel(Module):
+    cmd_vel: In[Twist]
+    config: AlfredHighLevelConfig
+
+    def __init__(self, *args: Any, **kwargs: Any) -> None:
+        super().__init__(*args, **kwargs)
+        self._client: portal.Client | None = None
+        self._stop_task: asyncio.Task[None] | None = None
+        self._last_velocities = [0.0, 0.0, 0.0]
+
+    async def main(self) -> AsyncGenerator[None, None]:
+        self._client = portal.Client(self.config.address)
+        logger.info(f"Connected to Alfred at {self.config.address}")
+        try:
+            yield
+        finally:
+            if self._stop_task is not None and not self._stop_task.done():
+                self._stop_task.cancel()
+            try:
+                await self._send_velocity(0.0, 0.0, 0.0)
+            except Exception as e:
+                logger.error(f"Error stopping Alfred: {e}")
+            if self._client is not None:
+                try:
+                    self._client.close()
+                except Exception:
+                    pass
+                self._client = None
+            logger.info("Alfred high-level connection stopped")
+
+    async def handle_cmd_vel(self, msg: Twist) -> None:
+        await self.move(msg)
+
+    @rpc
+    async def move(self, twist: Twist, duration: float = 0.0) -> bool:
+        """Send a Twist as a holonomic velocity command.
+
+        With ``duration > 0`` the command runs for that many seconds before
+        auto-stop. With ``duration == 0`` each call rearms a ``cmd_vel_timeout``
+        watchdog; if the stream stalls, the platform stops automatically.
+        """
+        if self._client is None:
+            logger.warning("Alfred not connected; ignoring move")
+            return False
+
+        vx, vy, wz = twist.linear.x, twist.linear.y, twist.angular.z
+
+        if self._stop_task is not None and not self._stop_task.done():
+            self._stop_task.cancel()
+
+        # Negate vy and wz for Alfred's inverted Y-axis frame.
+        # Send before scheduling the watchdog — otherwise it could fire first.
+        if not await self._send_velocity(vx, -vy, -wz):
+            return False
+
+        self._last_velocities = [vx, vy, wz]
+        timeout = duration if duration > 0 else self.config.cmd_vel_timeout
+        self._stop_task = asyncio.create_task(self._auto_stop_movement(timeout))
+        return True
+
+    async def _auto_stop_movement(self, delay: float) -> None:
+        try:
+            await asyncio.sleep(delay)
+        except asyncio.CancelledError:
+            return
+        try:
+            if 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}")
+
+    @rpc
+    async def get_state(self) -> str:
+        if self._client is None:
+            return "DISCONNECTED"
+        moving = any(abs(v) > 1e-6 for v in self._last_velocities)
+        return "MOVING" if moving else "STOPPED"
+
+    @skill
+    async def move_velocity(
+        self, x: float, y: float = 0.0, yaw: float = 0.0, duration: float = 0.0
+    ) -> str:
+        """Move the Alfred at the given velocity for ``duration`` seconds."""
+        twist = Twist(linear=Vector3(x, y, 0), angular=Vector3(0, 0, yaw))
+        await self.move(twist, duration=duration)
+        return f"Started moving with velocity=({x}, {y}, {yaw}) for {duration} seconds"
+
+    async def _send_velocity(self, vx: float, vy: float, wz: float) -> bool:
+        """Send a raw velocity (already in Alfred frame) via Portal RPC."""
+        if self._client is None:
+            return False
+        try:
+            command = {
+                "target_velocity": np.array([vx, vy, wz]),
+                "frame": "local",
+            }
+            future = self._client.set_target_velocity(command)
+            await asyncio.to_thread(future.result)
+            return True
+        except Exception as e:
+            logger.error(f"Error sending Alfred velocity: {e}")
+            return False
+
+
+__all__ = ["AlfredHighLevel", "AlfredHighLevelConfig"]

dimos/robot/test_all_blueprints.py

@@ -29,6 +29,7 @@
 # These need git LFS, so can't be run on the ubuntu runners.
 SELF_HOSTED_BLUEPRINTS = frozenset(
     {
+        "alfred-nav",
         "coordinator-basic",
         "coordinator-cartesian-ik-mock",
         "coordinator-cartesian-ik-piper",