diff --git a/dimos/agents/agent.py b/dimos/agents/agent.py index 37e1a4757c..e0f10848df 100644 --- a/dimos/agents/agent.py +++ b/dimos/agents/agent.py @@ -33,6 +33,9 @@ from dimos.core.stream import In, Out from dimos.protocol.rpc import RPCSpec from dimos.spec.utils import Spec +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() if TYPE_CHECKING: from langchain_core.language_models import BaseChatModel @@ -54,6 +57,7 @@ class Agent(Module[AgentConfig]): _lock: RLock _state_graph: CompiledStateGraph[Any, Any, Any, Any] | None _message_queue: Queue[BaseMessage] + _skill_registry: dict[str, SkillInfo] _history: list[BaseMessage] _thread: Thread _stop_event: Event @@ -64,6 +68,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._state_graph = None self._message_queue = Queue() self._history = [] + self._skill_registry = {} self._thread = Thread( target=self._thread_loop, name=f"{self.__class__.__name__}-thread", @@ -102,13 +107,16 @@ def on_system_modules(self, modules: list[RPCClient]) -> None: model = MockModel(json_path=self.config.model_fixture) + skills = [skill for module in modules for skill in (module.get_skills() or [])] + self._skill_registry = {skill.func_name: skill for skill in skills} + with self._lock: # Here to prevent unwanted imports in the file. from langchain.agents import create_agent self._state_graph = create_agent( model=model, - tools=_get_tools_from_modules(self, modules, self.rpc), + tools=[_skill_to_tool(self, skill, self.rpc) for skill in skills], system_prompt=self.config.system_prompt, ) self._thread.start() @@ -117,6 +125,64 @@ def on_system_modules(self, modules: list[RPCClient]) -> None: def add_message(self, message: BaseMessage) -> None: self._message_queue.put(message) + @rpc + def dispatch_continuation( + self, continuation: dict[str, Any], continuation_context: dict[str, Any] + ) -> None: + """Execute a tool continuation with detection data, bypassing the LLM. + + Called by trigger tools (e.g. look_out_for) to immediately invoke a + follow-up tool when a detection fires, without waiting for the LLM to + reason about the next action. + + Args: + continuation: ``{"tool": "", "args": {…}}`` — the tool to + call and its arguments. Argument values that are strings + starting with ``$`` are treated as template variables and + resolved against *continuation_context* (e.g. ``"$bbox"``). + continuation_context: runtime detection data, e.g. + ``{"bbox": [x1, y1, x2, y2], "label": "person"}``. + """ + tool_name = continuation.get("tool") + if not tool_name: + self._message_queue.put( + HumanMessage(f"Continuation failed: missing 'tool' key in {continuation}") + ) + return + + skill_info = self._skill_registry.get(tool_name) + if skill_info is None: + self._message_queue.put( + HumanMessage(f"Continuation failed: tool '{tool_name}' not found") + ) + return + + tool_args: dict[str, Any] = dict(continuation.get("args", {})) + + # Substitute $-prefixed template variables from continuation_context + for key, value in tool_args.items(): + if isinstance(value, str) and value.startswith("$"): + context_key = value[1:] + if context_key in continuation_context: + tool_args[key] = continuation_context[context_key] + + rpc_call = RpcCall(None, self.rpc, skill_info.func_name, skill_info.class_name, []) + try: + result = rpc_call(**tool_args) + except Exception as e: + self._message_queue.put( + HumanMessage(f"Continuation '{tool_name}' failed with error: {e}") + ) + return + + label = continuation_context.get("label", "unknown") + self._message_queue.put( + HumanMessage( + f"Automatically executed '{tool_name}' as a continuation of lookout " + f"detection (detected: {label}). Result: {result or 'started'}" + ) + ) + def _thread_loop(self) -> None: while not self._stop_event.is_set(): try: @@ -150,13 +216,9 @@ def _process_message( class AgentSpec(Spec, Protocol): def add_message(self, message: BaseMessage) -> None: ... - - -def _get_tools_from_modules( - agent: Agent, modules: list[RPCClient], rpc: RPCSpec -) -> list[StructuredTool]: - skills = [skill for module in modules for skill in (module.get_skills() or [])] - return [_skill_to_tool(agent, skill, rpc) for skill in skills] + def dispatch_continuation( + self, continuation: dict[str, Any], continuation_context: dict[str, Any] + ) -> None: ... def _skill_to_tool(agent: Agent, skill: SkillInfo, rpc: RPCSpec) -> StructuredTool: diff --git a/dimos/agents/mcp/mcp_client.py b/dimos/agents/mcp/mcp_client.py index 7c5eda5302..f0b0e1a121 100644 --- a/dimos/agents/mcp/mcp_client.py +++ b/dimos/agents/mcp/mcp_client.py @@ -56,6 +56,7 @@ class McpClient(Module[McpClientConfig]): _lock: RLock _state_graph: CompiledStateGraph[Any, Any, Any, Any] | None _message_queue: Queue[BaseMessage] + _tool_registry: dict[str, dict[str, Any]] _history: list[BaseMessage] _thread: Thread _stop_event: Event @@ -67,6 +68,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._lock = RLock() self._state_graph = None self._message_queue = Queue() + self._tool_registry = {} self._history = [] self._thread = Thread( target=self._thread_loop, @@ -106,7 +108,9 @@ def _fetch_tools(self, timeout: float = 60.0, interval: float = 1.0) -> list[Str f"Failed to fetch tools from MCP server {self.config.mcp_server_url}" ) - tools = [self._mcp_tool_to_langchain(t) for t in result.get("tools", [])] + raw_tools = result.get("tools", []) + self._tool_registry = {t["name"]: t for t in raw_tools} + tools = [self._mcp_tool_to_langchain(t) for t in raw_tools] if not tools: logger.warning("No tools found from MCP server.") @@ -198,6 +202,65 @@ def stop(self) -> None: def add_message(self, message: BaseMessage) -> None: self._message_queue.put(message) + @rpc + def dispatch_continuation( + self, continuation: dict[str, Any], continuation_context: dict[str, Any] + ) -> None: + """Execute a tool continuation with detection data, bypassing the LLM. + + Called by trigger tools (e.g. look_out_for) to immediately invoke a + follow-up tool when a detection fires, without waiting for the LLM to + reason about the next action. + + Args: + continuation: ``{"tool": "", "args": {…}}`` — the tool to + call and its arguments. Argument values that are strings + starting with ``$`` are treated as template variables and + resolved against *continuation_context* (e.g. ``"$bbox"``). + continuation_context: runtime detection data, e.g. + ``{"bbox": [x1, y1, x2, y2], "label": "person"}``. + """ + tool_name = continuation.get("tool") + if not tool_name: + self._message_queue.put( + HumanMessage(f"Continuation failed: missing 'tool' key in {continuation}") + ) + return + + if tool_name not in self._tool_registry: + self._message_queue.put( + HumanMessage(f"Continuation failed: tool '{tool_name}' not found") + ) + return + + tool_args: dict[str, Any] = dict(continuation.get("args", {})) + + # Substitute $-prefixed template variables from continuation_context + for key, value in tool_args.items(): + if isinstance(value, str) and value.startswith("$"): + context_key = value[1:] + if context_key in continuation_context: + tool_args[key] = continuation_context[context_key] + + try: + result = self._mcp_request("tools/call", {"name": tool_name, "arguments": tool_args}) + content = result.get("content", []) + parts = [c.get("text", "") for c in content if c.get("type") == "text"] + text = "\n".join(parts) + except Exception as e: + self._message_queue.put( + HumanMessage(f"Continuation '{tool_name}' failed with error: {e}") + ) + return + + label = continuation_context.get("label", "unknown") + self._message_queue.put( + HumanMessage( + f"Automatically executed '{tool_name}' as a continuation of lookout " + f"detection (detected: {label}). Result: {text or 'started'}" + ) + ) + def _thread_loop(self) -> None: while not self._stop_event.is_set(): try: diff --git a/dimos/agents/skills/person_follow.py b/dimos/agents/skills/person_follow.py index e59ddb3b2a..1029f51388 100644 --- a/dimos/agents/skills/person_follow.py +++ b/dimos/agents/skills/person_follow.py @@ -12,6 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. +import base64 from threading import Event, RLock, Thread import time from typing import TYPE_CHECKING @@ -19,6 +20,7 @@ from langchain_core.messages import HumanMessage import numpy as np from reactivex.disposable import Disposable +from turbojpeg import TurboJPEG from dimos.agents.agent import AgentSpec from dimos.agents.annotation import skill @@ -29,7 +31,8 @@ from dimos.models.qwen.bbox import BBox from dimos.models.vl.create import create from dimos.msgs.geometry_msgs import Twist -from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image, ImageFormat, PointCloud2 +from dimos.navigation.patrolling.patrolling_module_spec import PatrollingModuleSpec from dimos.navigation.visual.query import get_object_bbox_from_image from dimos.navigation.visual_servoing.detection_navigation import DetectionNavigation from dimos.navigation.visual_servoing.visual_servoing_2d import VisualServoing2D @@ -59,6 +62,7 @@ class PersonFollowSkillContainer(Module): _agent_spec: AgentSpec _frequency: float = 20.0 # Hz - control loop frequency _max_lost_frames: int = 15 # number of frames to wait before declaring person lost + _patrolling_module_spec: PatrollingModuleSpec def __init__( self, @@ -107,7 +111,12 @@ def stop(self) -> None: super().stop() @skill - def follow_person(self, query: str) -> str: + def follow_person( + self, + query: str, + initial_bbox: list[float] | None = None, + initial_image: str | None = None, + ) -> str: """Follow a person matching the given description using visual servoing. The robot will continuously track and follow the person, while keeping @@ -115,6 +124,12 @@ def follow_person(self, query: str) -> str: Args: query: Description of the person to follow (e.g., "man with blue shirt") + initial_bbox: Optional pre-computed bounding box [x1, y1, x2, y2]. + If provided, skips the initial VL model detection step. This is + used by the continuation system to pass detection data directly + from look_out_for, avoiding a redundant detection. + initial_image: Optional base64-encoded JPEG of the frame on which + initial_bbox was detected. Returns: Status message indicating the result of the following action. @@ -134,16 +149,27 @@ def follow_person(self, query: str) -> str: if latest_image is None: return "No image available to detect person." - initial_bbox = get_object_bbox_from_image( - self._vl_model, - latest_image, - query, - ) - - if initial_bbox is None: - return f"Could not find '{query}' in the current view." - - return self._follow_person(query, initial_bbox) + detection_image: Image | None = None + if initial_bbox is not None: + bbox: BBox = ( + initial_bbox[0], + initial_bbox[1], + initial_bbox[2], + initial_bbox[3], + ) + if initial_image is not None: + detection_image = _decode_base64_image(initial_image) + else: + detected = get_object_bbox_from_image( + self._vl_model, + latest_image, + query, + ) + if detected is None: + return f"Could not find '{query}' in the current view." + bbox = detected + + return self._follow_person(query, bbox, detection_image) @skill def stop_following(self) -> str: @@ -170,7 +196,9 @@ def _on_pointcloud(self, pointcloud: PointCloud2) -> None: with self._lock: self._latest_pointcloud = pointcloud - def _follow_person(self, query: str, initial_bbox: BBox) -> str: + def _follow_person( + self, query: str, initial_bbox: BBox, detection_image: Image | None = None + ) -> str: x1, y1, x2, y2 = initial_bbox box = np.array([x1, y1, x2, y2], dtype=np.float32) @@ -185,8 +213,11 @@ def _follow_person(self, query: str, initial_bbox: BBox) -> str: if latest_image is None: return "No image available to start tracking." + # Use the detection frame for tracker init when available, so the bbox + # matches the image it was computed on. + init_image = detection_image if detection_image is not None else latest_image initial_detections = tracker.init_track( - image=latest_image, + image=init_image, box=box, obj_id=1, ) @@ -200,11 +231,21 @@ def _follow_person(self, query: str, initial_bbox: BBox) -> str: self._thread = Thread(target=self._follow_loop, args=(tracker, query), daemon=True) self._thread.start() - return ( + message = ( "Found the person. Starting to follow. You can stop following by calling " "the 'stop_following' tool." ) + if self._patrolling_module_spec.is_patrolling(): + message += ( + " Note: since the robot was patrolling, this has been stopped automatically " + "(the equivalent of calling the `stop_patrol` tool call) so you don't have " + "to do it. " + ) + self._patrolling_module_spec.stop_patrol() + + return message + def _follow_loop(self, tracker: "EdgeTAMProcessor", query: str) -> None: lost_count = 0 period = 1.0 / self._frequency @@ -268,6 +309,11 @@ def _send_stop_reason(self, query: str, reason: str) -> None: logger.info("Person follow stopped", query=query, reason=reason) +def _decode_base64_image(b64: str) -> Image: + bgr_array = TurboJPEG().decode(base64.b64decode(b64)) + return Image(data=bgr_array, format=ImageFormat.BGR) + + person_follow_skill = PersonFollowSkillContainer.blueprint __all__ = ["PersonFollowSkillContainer", "person_follow_skill"] diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 60072ae7fd..0b070dabd9 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -17,7 +17,6 @@ from pydantic_settings import BaseSettings, SettingsConfigDict -from dimos.mapping.occupancy.path_map import NavigationStrategy from dimos.models.vl.create import VlModelName ViewerBackend: TypeAlias = Literal["rerun", "rerun-web", "rerun-connect", "foxglove", "none"] @@ -47,7 +46,7 @@ class GlobalConfig(BaseSettings): robot_model: str | None = None robot_width: float = 0.3 robot_rotation_diameter: float = 0.6 - planner_strategy: NavigationStrategy = "simple" + nerf_speed: float = 1.0 planner_robot_speed: float | None = None mcp_port: int = 9990 mcp_host: str = "0.0.0.0" diff --git a/dimos/e2e_tests/conftest.py b/dimos/e2e_tests/conftest.py index 51ab7c2c18..fa0ffb1fdc 100644 --- a/dimos/e2e_tests/conftest.py +++ b/dimos/e2e_tests/conftest.py @@ -25,6 +25,7 @@ from dimos.msgs.geometry_msgs import PoseStamped, Quaternion from dimos.msgs.geometry_msgs.Vector3 import make_vector3 from dimos.msgs.std_msgs.Bool import Bool +from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer from dimos.simulation.mujoco.person_on_track import PersonTrackPublisher @@ -115,3 +116,11 @@ def run_person_track() -> None: thread.join(timeout=1.0) if publisher is not None: publisher.stop() + + +@pytest.fixture +def direct_cmd_vel_explorer() -> Generator[PersonTrackPublisher, None, None]: + explorer = DirectCmdVelExplorer() + explorer.start() + yield explorer + explorer.stop() diff --git a/dimos/e2e_tests/test_patrol_and_follow.py b/dimos/e2e_tests/test_patrol_and_follow.py new file mode 100644 index 0000000000..b89da3a0b7 --- /dev/null +++ b/dimos/e2e_tests/test_patrol_and_follow.py @@ -0,0 +1,89 @@ +# Copyright 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 collections.abc import Callable +import time + +import pytest + +from dimos.e2e_tests.conf_types import StartPersonTrack +from dimos.e2e_tests.dimos_cli_call import DimosCliCall +from dimos.e2e_tests.lcm_spy import LcmSpy +from dimos.simulation.mujoco.direct_cmd_vel_explorer import DirectCmdVelExplorer + +points = [ + (0, -7.07), + (-4.16, -7.07), + (-4.45, 1.10), + (-6.72, 2.87), + (-1.78, 3.01), + (-1.54, 5.74), + (3.88, 6.16), + (2.16, 9.36), + (4.70, 3.87), + (4.67, -7.15), + (4.57, -4.19), + (-0.84, -2.78), + (-4.71, 1.17), + (4.30, 0.87), +] + + +@pytest.mark.skipif_in_ci +@pytest.mark.skipif_no_openai +@pytest.mark.mujoco +def test_patrol_and_follow( + lcm_spy: LcmSpy, + start_blueprint: Callable[[str], DimosCliCall], + human_input: Callable[[str], None], + start_person_track: StartPersonTrack, + direct_cmd_vel_explorer: DirectCmdVelExplorer, +) -> None: + start_blueprint( + "--mujoco-start-pos", + "-10.75 -6.78", + "--nerf-speed", + "0.3", + "run", + "--disable", + "spatial-memory", + "unitree-go2-agentic", + ) + + lcm_spy.save_topic("/rpc/Agent/on_system_modules/res") + lcm_spy.wait_for_saved_topic("/rpc/Agent/on_system_modules/res", timeout=120.0) + + time.sleep(5) + + print("Starting discovery.") + + # Explore the entire room by driving directly via /cmd_vel. + direct_cmd_vel_explorer.follow_points(points) + + print("Ended discovery.") + + start_person_track( + [ + (-3.35, -0.51), + (-2.60, 1.28), + (4.80, 0.21), + (4.14, -6.0), + (0.59, -3.79), + ] + ) + human_input( + "patrol around until you find a person (any person) and when you do, start following him" + ) + + time.sleep(120) diff --git a/dimos/mapping/occupancy/gradient.py b/dimos/mapping/occupancy/gradient.py index 880f2692da..1e2c7a7221 100644 --- a/dimos/mapping/occupancy/gradient.py +++ b/dimos/mapping/occupancy/gradient.py @@ -12,11 +12,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +from typing import TYPE_CHECKING, Literal, TypeAlias, cast + import numpy as np from scipy import ndimage # type: ignore[import-untyped] from dimos.msgs.nav_msgs.OccupancyGrid import CostValues, OccupancyGrid +if TYPE_CHECKING: + from numpy.typing import NDArray + + +GradientStrategy: TypeAlias = Literal["gradient", "voronoi"] + def gradient( occupancy_grid: OccupancyGrid, obstacle_threshold: int = 50, max_distance: float = 2.0 @@ -50,7 +58,7 @@ def gradient( # Compute distance transform (distance to nearest obstacle in cells) # Unknown cells are treated as if they don't exist for distance calculation - distance_cells = ndimage.distance_transform_edt(1 - obstacle_map) + distance_cells = cast("NDArray[np.float64]", ndimage.distance_transform_edt(1 - obstacle_map)) # Convert to meters and clip to max distance distance_meters = np.clip(distance_cells * occupancy_grid.resolution, 0, max_distance) diff --git a/dimos/mapping/occupancy/path_map.py b/dimos/mapping/occupancy/path_map.py index a99a423de8..8920c6e30b 100644 --- a/dimos/mapping/occupancy/path_map.py +++ b/dimos/mapping/occupancy/path_map.py @@ -14,7 +14,7 @@ from typing import Literal, TypeAlias -from dimos.mapping.occupancy.gradient import voronoi_gradient +from dimos.mapping.occupancy.gradient import GradientStrategy, gradient, voronoi_gradient from dimos.mapping.occupancy.inflation import simple_inflate from dimos.mapping.occupancy.operations import overlay_occupied, smooth_occupied from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid @@ -23,7 +23,10 @@ def make_navigation_map( - occupancy_grid: OccupancyGrid, robot_width: float, strategy: NavigationStrategy + occupancy_grid: OccupancyGrid, + robot_width: float, + strategy: NavigationStrategy, + gradient_strategy: GradientStrategy, ) -> OccupancyGrid: half_width = robot_width / 2 gradient_distance = 1.5 @@ -37,4 +40,9 @@ def make_navigation_map( else: raise ValueError(f"Unknown strategy: {strategy}") - return voronoi_gradient(costmap, max_distance=gradient_distance) + if gradient_strategy == "gradient": + return gradient(costmap, max_distance=gradient_distance) + elif gradient_strategy == "voronoi": + return voronoi_gradient(costmap, max_distance=gradient_distance) + else: + raise ValueError(f"Unknown gradient strategy: {gradient_strategy}") diff --git a/dimos/mapping/occupancy/test_path_map.py b/dimos/mapping/occupancy/test_path_map.py index b3e250db9d..8928e1ab92 100644 --- a/dimos/mapping/occupancy/test_path_map.py +++ b/dimos/mapping/occupancy/test_path_map.py @@ -28,7 +28,7 @@ def test_make_navigation_map(occupancy, strategy) -> None: expected = cv2.imread(get_data(f"make_navigation_map_{strategy}.png"), cv2.IMREAD_COLOR) robot_width = 0.4 - og = make_navigation_map(occupancy, robot_width, strategy=strategy) + og = make_navigation_map(occupancy, robot_width, strategy=strategy, gradient_strategy="voronoi") result = visualize_occupancy_grid(og, "rainbow") np.testing.assert_array_equal(result.data, expected) diff --git a/dimos/models/segmentation/edge_tam.py b/dimos/models/segmentation/edge_tam.py index 54158b2b92..b0f7ceaada 100644 --- a/dimos/models/segmentation/edge_tam.py +++ b/dimos/models/segmentation/edge_tam.py @@ -38,7 +38,6 @@ if TYPE_CHECKING: from sam2.sam2_video_predictor import SAM2VideoPredictor -os.environ['TQDM_DISABLE'] = '1' logger = setup_logger() @@ -88,6 +87,10 @@ def __init__( self._predictor = instantiate(cfg.model, _recursive_=True) + # Suppress the per-frame "propagate in video" tqdm bar from sam2 + import sam2.sam2_video_predictor as _svp + _svp.tqdm = lambda iterable, *a, **kw: iterable + ckpt_path = str(get_data("models_edgetam") / "edgetam.pt") sd = torch.load(ckpt_path, map_location="cpu", weights_only=True)["model"] diff --git a/dimos/navigation/patrolling/create_patrol_router.py b/dimos/navigation/patrolling/create_patrol_router.py new file mode 100644 index 0000000000..b26e8b4edf --- /dev/null +++ b/dimos/navigation/patrolling/create_patrol_router.py @@ -0,0 +1,45 @@ +# Copyright 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 typing import TYPE_CHECKING, Literal + +if TYPE_CHECKING: + from dimos.navigation.patrolling.routers.patrol_router import PatrolRouter + +PatrolRouterName = Literal["random", "coverage", "frontier"] + + +def create_patrol_router(name: PatrolRouterName, clearance_radius_m: float) -> PatrolRouter: + match name: + case "random": + # Inline to avoid unnecessary imports. + from dimos.navigation.patrolling.routers.random_patrol_router import RandomPatrolRouter + + return RandomPatrolRouter(clearance_radius_m) + case "coverage": + # Inline to avoid unnecessary imports. + from dimos.navigation.patrolling.routers.coverage_patrol_router import ( + CoveragePatrolRouter, + ) + + return CoveragePatrolRouter(clearance_radius_m) + case "frontier": + # Inline to avoid unnecessary imports. + from dimos.navigation.patrolling.routers.frontier_patrol_router import ( + FrontierPatrolRouter, + ) + + return FrontierPatrolRouter(clearance_radius_m) diff --git a/dimos/navigation/patrolling/module.py b/dimos/navigation/patrolling/module.py new file mode 100644 index 0000000000..9fd3c65ad4 --- /dev/null +++ b/dimos/navigation/patrolling/module.py @@ -0,0 +1,141 @@ +# Copyright 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. + + +import threading + +from dimos_lcm.std_msgs import Bool +from reactivex.disposable import Disposable + +from dimos.agents.annotation import skill +from dimos.core.core import rpc +from dimos.core.global_config import GlobalConfig, global_config +from dimos.core.module import Module +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs import OccupancyGrid +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.routers.patrol_router import PatrolRouter +from dimos.navigation.replanning_a_star.module_spec import ReplanningAStarPlannerSpec +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class PatrollingModule(Module): + odom: In[PoseStamped] + global_costmap: In[OccupancyGrid] + goal_reached: In[Bool] + goal_request: Out[PoseStamped] + + _global_config: GlobalConfig + _router: PatrolRouter + _planner_spec: ReplanningAStarPlannerSpec + + _clearance_multiplier = 0.5 + + def __init__(self, cfg: GlobalConfig = global_config) -> None: + super().__init__() + self._global_config = cfg + clearance_radius_m = self._global_config.robot_width * self._clearance_multiplier + self._router = create_patrol_router("coverage", clearance_radius_m) + + self._patrol_lock = threading.RLock() + self._patrol_thread: threading.Thread | None = None + self._stop_event = threading.Event() + self._goal_reached_event = threading.Event() + self._goal_or_stop_event = threading.Event() + self._latest_pose: PoseStamped | None = None + + @rpc + def start(self) -> None: + super().start() + + self._disposables.add(Disposable(self.odom.subscribe(self._on_odom))) + self._disposables.add( + Disposable(self.global_costmap.subscribe(self._router.handle_occupancy_grid)) + ) + self._disposables.add(Disposable(self.goal_reached.subscribe(self._on_goal_reached))) + + @rpc + def stop(self) -> None: + self._stop_patrolling() + super().stop() + + @skill + def start_patrol(self) -> str: + """Start patrolling the known area. The robot will continuously pick patrol goals from the router and navigate to them until `stop_patrol` is called.""" + with self._patrol_lock: + if self._patrol_thread is not None and self._patrol_thread.is_alive(): + return "Patrol is already running. Use `stop_patrol` to stop." + self._planner_spec.set_replanning_enabled(False) + self._planner_spec.set_safe_goal_clearance( + self._global_config.robot_rotation_diameter / 2 + 0.2 + ) + self._stop_event.clear() + self._patrol_thread = threading.Thread( + target=self._patrol_loop, daemon=True, name=self.__class__.__name__ + ) + self._patrol_thread.start() + return "Patrol started. Use `stop_patrol` to stop." + + @rpc + def is_patrolling(self) -> bool: + with self._patrol_lock: + return self._patrol_thread is not None and self._patrol_thread.is_alive() + + @skill + def stop_patrol(self) -> str: + """Stop the ongoing patrol.""" + self._stop_patrolling() + return "Patrol stopped." + + def _on_odom(self, msg: PoseStamped) -> None: + self._latest_pose = msg + self._router.handle_odom(msg) + + def _on_goal_reached(self, _msg: Bool) -> None: + self._goal_reached_event.set() + self._goal_or_stop_event.set() + + def _patrol_loop(self) -> None: + while not self._stop_event.is_set(): + goal = self._router.next_goal() + if goal is None: + logger.info("No patrol goal available, retrying in 2s") + if self._stop_event.wait(timeout=2.0): + break + continue + + self._goal_reached_event.clear() + self.goal_request.publish(goal) + + # Wait until goal is reached or stop is requested. + self._goal_or_stop_event.wait() + self._goal_or_stop_event.clear() + + def _stop_patrolling(self) -> None: + self._stop_event.set() + self._goal_or_stop_event.set() + self._planner_spec.set_replanning_enabled(True) + self._planner_spec.reset_safe_goal_clearance() + + # Publish current position as goal to cancel in-progress navigation. + pose = self._latest_pose + if pose is not None: + self.goal_request.publish(pose) + with self._patrol_lock: + if self._patrol_thread is not None: + self._patrol_thread.join() + self._patrol_thread = None diff --git a/dimos/navigation/patrolling/patrolling_module_spec.py b/dimos/navigation/patrolling/patrolling_module_spec.py new file mode 100644 index 0000000000..23dffeec16 --- /dev/null +++ b/dimos/navigation/patrolling/patrolling_module_spec.py @@ -0,0 +1,27 @@ +# Copyright 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 typing import Protocol + +from dimos.spec.utils import Spec +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class PatrollingModuleSpec(Spec, Protocol): + def start_patrol(self) -> str: ... + def is_patrolling(self) -> bool: ... + def stop_patrol(self) -> str: ... diff --git a/dimos/navigation/patrolling/routers/base_patrol_router.py b/dimos/navigation/patrolling/routers/base_patrol_router.py new file mode 100644 index 0000000000..37bbad333d --- /dev/null +++ b/dimos/navigation/patrolling/routers/base_patrol_router.py @@ -0,0 +1,72 @@ +# Copyright 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 abc import ABC, abstractmethod +from threading import RLock +import time + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.navigation.patrolling.routers.visitation_history import VisitationHistory + + +class BasePatrolRouter(ABC): + _occupancy_grid_min_update_interval_s = 60.0 + _occupancy_grid: OccupancyGrid | None + _occupancy_grid_updated_at: float + _pose: PoseStamped | None + _lock: RLock + _clearance_radius_m: float + + def __init__(self, clearance_radius_m: float) -> None: + self._occupancy_grid = None + self._occupancy_grid_updated_at = 0.0 + self._visitation = VisitationHistory(clearance_radius_m) + self._pose = None + self._lock = RLock() + self._clearance_radius_m = clearance_radius_m + + @property + def _visited(self) -> NDArray[np.bool_] | None: + return self._visitation.visited + + def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: + with self._lock: + now = time.monotonic() + if ( + self._occupancy_grid is not None + and now - self._occupancy_grid_updated_at + < self._occupancy_grid_min_update_interval_s + ): + return + self._occupancy_grid = msg + self._occupancy_grid_updated_at = now + self._visitation.update_grid(msg) + + def handle_odom(self, msg: PoseStamped) -> None: + with self._lock: + self._pose = msg + if self._occupancy_grid is None: + return + self._visitation.handle_odom(msg.position.x, msg.position.y) + + def get_saturation(self) -> float: + with self._lock: + return self._visitation.get_saturation() + + @abstractmethod + def next_goal(self) -> PoseStamped | None: ... diff --git a/dimos/navigation/patrolling/routers/coverage_patrol_router.py b/dimos/navigation/patrolling/routers/coverage_patrol_router.py new file mode 100644 index 0000000000..1b7ee6a4ac --- /dev/null +++ b/dimos/navigation/patrolling/routers/coverage_patrol_router.py @@ -0,0 +1,164 @@ +# Copyright 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. + +import numpy as np +from numpy.typing import NDArray +from scipy.ndimage import binary_erosion + +from dimos.mapping.occupancy.gradient import gradient, voronoi_gradient +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs import Path +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter +from dimos.navigation.patrolling.utilities import point_to_pose_stamped +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar + + +class CoveragePatrolRouter(BasePatrolRouter): + _costmap: OccupancyGrid | None + _safe_mask: NDArray[np.bool_] | None + _sampling_weights: NDArray[np.float64] | None + _candidates_to_consider: int = 7 + + def __init__(self, clearance_radius_m: float) -> None: + super().__init__(clearance_radius_m) + self._costmap = None + self._safe_mask = None + self._sampling_weights = None + + def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: + with self._lock: + prev = self._occupancy_grid + super().handle_occupancy_grid(msg) + if self._occupancy_grid is prev: + # Throttled — no update happened. + return + self._costmap = gradient(msg, max_distance=1.5) + + # Precompute the safe mask (cells with enough clearance from obstacles). + clearance_cells = self._visitation.clearance_radius_cells + free_mask = msg.grid == 0 + structure = np.ones((2 * clearance_cells + 1, 2 * clearance_cells + 1), dtype=bool) + self._safe_mask = binary_erosion(free_mask, structure=structure).astype(bool) + + # Precompute voronoi-based sampling weights so candidates are spread + # across different corridors/regions rather than clustering in large + # open areas. Low voronoi cost = on the skeleton (equidistant from + # walls) = high sampling weight. + voronoi = voronoi_gradient(msg, max_distance=1.5) + voronoi_cost = voronoi.grid.astype(np.float64) + # Invert: skeleton cells (cost 0) become weight 100, walls (100) become 0. + # Clamp negatives (unknown = -1) to 0. + weights = np.clip(100.0 - voronoi_cost, 0.0, 100.0) + self._sampling_weights = weights + + def next_goal(self) -> PoseStamped | None: + with self._lock: + if ( + self._occupancy_grid is None + or self._visited is None + or self._safe_mask is None + or self._costmap is None + or self._sampling_weights is None + ): + return None + occupancy_grid = self._occupancy_grid + costmap = self._costmap + safe_mask = self._safe_mask + sampling_weights = self._sampling_weights + visited = self._visited.copy() + pose = self._pose + + if pose is None: + return None + + start = (pose.position.x, pose.position.y) + + # Get candidate points from unvisited safe cells. + unvisited_safe = safe_mask & ~visited + if not np.any(unvisited_safe): + # Fall back to all safe cells if everything visited. + unvisited_safe = safe_mask + if not np.any(unvisited_safe): + return None + + safe_indices = np.argwhere(unvisited_safe) + n_candidates = min(self._candidates_to_consider, len(safe_indices)) + + # Weight candidates by voronoi score so they spread across corridors + # rather than clustering in large open areas. + weights = sampling_weights[safe_indices[:, 0], safe_indices[:, 1]] + weight_sum = weights.sum() + if weight_sum > 0: + probs = weights / weight_sum + else: + probs = None + chosen = safe_indices[ + np.random.choice(len(safe_indices), size=n_candidates, replace=False, p=probs) + ] + + best_score = -1 + best_point = None + + for row, col in chosen: + world = occupancy_grid.grid_to_world((col, row, 0)) + candidate = (world.x, world.y) + + path = min_cost_astar(costmap, candidate, start, unknown_penalty=1.0, use_cpp=True) + if path is None: + continue + + # Count how many new (unvisited) cells would be covered along this path. + new_cells = self._count_new_coverage(path, visited, occupancy_grid, safe_mask) + if new_cells > best_score: + best_score = new_cells + best_point = candidate + + if best_point is None: + return None + return point_to_pose_stamped(best_point) + + def _count_new_coverage( + self, + path: Path, + visited: NDArray[np.bool_], + occupancy_grid: OccupancyGrid, + safe_mask: NDArray[np.bool_], + ) -> int: + r = self._visitation.clearance_radius_cells + h, w = visited.shape + covered = np.zeros_like(visited) + + # Sample every few poses to avoid redundant work on dense paths. + step = max(1, r) + poses = path.poses[::step] + + for pose in poses: + grid = occupancy_grid.world_to_grid((pose.position.x, pose.position.y)) + col, row = int(grid.x), int(grid.y) + r_min = max(0, row - r) + r_max = min(h, row + r + 1) + c_min = max(0, col - r) + c_max = min(w, col + r + 1) + d_r_min = r_min - (row - r) + d_r_max = d_r_min + (r_max - r_min) + d_c_min = c_min - (col - r) + d_c_max = d_c_min + (c_max - c_min) + covered[r_min:r_max, c_min:c_max] |= self._visitation.clearance_disk[ + d_r_min:d_r_max, d_c_min:d_c_max + ] + + # New coverage = cells in covered that are not yet visited and are free space. + new = covered & ~visited & safe_mask + return int(np.count_nonzero(new)) diff --git a/dimos/navigation/patrolling/routers/frontier_patrol_router.py b/dimos/navigation/patrolling/routers/frontier_patrol_router.py new file mode 100644 index 0000000000..daac86a765 --- /dev/null +++ b/dimos/navigation/patrolling/routers/frontier_patrol_router.py @@ -0,0 +1,125 @@ +# Copyright 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 typing import Any + +import numpy as np +from numpy.typing import NDArray +from scipy.ndimage import binary_erosion, label + +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter +from dimos.navigation.patrolling.utilities import point_to_pose_stamped + + +class FrontierPatrolRouter(BasePatrolRouter): + """Patrol router that picks goals based on unvisited frontier clusters. + + This router: + 1. Finds connected components of unvisited safe cells. + 2. Scores each component by size / euclidean_distance from the robot. + 3. Within the best component, picks the point farthest from the robot + to create long sweeping paths through unvisited territory. + """ + + _safe_mask: NDArray[np.bool_] | None + _min_cluster_cells: int = 20 + + def __init__(self, clearance_radius_m: float) -> None: + super().__init__(clearance_radius_m) + self._safe_mask = None + + def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: + with self._lock: + prev = self._occupancy_grid + super().handle_occupancy_grid(msg) + if self._occupancy_grid is prev: + return + + clearance_cells = self._visitation.clearance_radius_cells + free_mask = msg.grid == 0 + structure = np.ones((2 * clearance_cells + 1, 2 * clearance_cells + 1), dtype=bool) + self._safe_mask = binary_erosion(free_mask, structure=structure).astype(bool) + + def next_goal(self) -> PoseStamped | None: + with self._lock: + if self._occupancy_grid is None or self._visited is None or self._safe_mask is None: + return None + occupancy_grid = self._occupancy_grid + safe_mask = self._safe_mask + visited = self._visited.copy() + pose = self._pose + + if pose is None: + return None + + # Robot position in grid coordinates. + grid_pos = occupancy_grid.world_to_grid((pose.position.x, pose.position.y)) + robot_col, robot_row = grid_pos.x, grid_pos.y + + # Unvisited safe cells. + unvisited_safe = safe_mask & ~visited + if not np.any(unvisited_safe): + unvisited_safe = safe_mask + if not np.any(unvisited_safe): + return None + + # Find connected components of unvisited safe space. + labeled, n_components = label(unvisited_safe) + if n_components == 0: + return None + + # Compute size and centroid of each component using vectorized ops. + component_ids = np.arange(1, n_components + 1) + rows, cols = np.where(labeled > 0) + labels_flat = labeled[rows, cols] + + # Size and centroid of each component. + sizes = np.bincount(labels_flat, minlength=n_components + 1)[1:] + sum_rows = np.bincount(labels_flat, weights=rows, minlength=n_components + 1)[1:] + sum_cols = np.bincount(labels_flat, weights=cols, minlength=n_components + 1)[1:] + + # Filter out tiny clusters. + valid = sizes >= self._min_cluster_cells + if not np.any(valid): + valid = sizes > 0 + + valid_ids = component_ids[valid] + valid_sizes = sizes[valid].astype(np.float64) + + # Euclidean distance from robot to each cluster centroid. + centroid_rows = sum_rows[valid] / valid_sizes + centroid_cols = sum_cols[valid] / valid_sizes + dr = centroid_rows - robot_row + dc = centroid_cols - robot_col + distances = np.maximum(np.sqrt(dr * dr + dc * dc), 1.0) + + # Score: prefer large, nearby clusters. + scores = valid_sizes / distances + + best_idx = int(np.argmax(scores)) + + # Within the best cluster, pick the point farthest from the robot. + # This creates long sweeping paths through unvisited territory instead + # of tiny movements toward a barely-shifting centroid. + cluster_mask = labeled == valid_ids[best_idx] + cluster_indices = np.argwhere(cluster_mask) + cluster_dr: NDArray[np.floating[Any]] = cluster_indices[:, 0] - robot_row + cluster_dc: NDArray[np.floating[Any]] = cluster_indices[:, 1] - robot_col + dists_sq = cluster_dr * cluster_dr + cluster_dc * cluster_dc + goal_row, goal_col = cluster_indices[np.argmax(dists_sq)] + + world = occupancy_grid.grid_to_world((int(goal_col), int(goal_row), 0)) + return point_to_pose_stamped((world.x, world.y)) diff --git a/dimos/navigation/patrolling/routers/patrol_router.py b/dimos/navigation/patrolling/routers/patrol_router.py new file mode 100644 index 0000000000..ebbf551708 --- /dev/null +++ b/dimos/navigation/patrolling/routers/patrol_router.py @@ -0,0 +1,26 @@ +# Copyright 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 typing import Protocol + +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid + + +class PatrolRouter(Protocol): + def __init__(self, clearance_radius_m: float) -> None: ... + def handle_occupancy_grid(self, msg: OccupancyGrid) -> None: ... + def handle_odom(self, msg: PoseStamped) -> None: ... + def next_goal(self) -> PoseStamped | None: ... + def get_saturation(self) -> float: ... diff --git a/dimos/navigation/patrolling/routers/random_patrol_router.py b/dimos/navigation/patrolling/routers/random_patrol_router.py new file mode 100644 index 0000000000..8973ac4530 --- /dev/null +++ b/dimos/navigation/patrolling/routers/random_patrol_router.py @@ -0,0 +1,68 @@ +# Copyright 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. + +import numpy as np +from numpy.typing import NDArray +from scipy.ndimage import binary_erosion + +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.navigation.patrolling.routers.base_patrol_router import BasePatrolRouter +from dimos.navigation.patrolling.utilities import point_to_pose_stamped + + +class RandomPatrolRouter(BasePatrolRouter): + def next_goal(self) -> PoseStamped | None: + with self._lock: + if self._occupancy_grid is None or self._visited is None: + return None + occupancy_grid = self._occupancy_grid + visited = self._visited.copy() + point = _random_empty_spot( + occupancy_grid, clearance_m=self._clearance_radius_m, visited=visited + ) + if point is None: + return None + return point_to_pose_stamped(point) + + +def _random_empty_spot( + occupancy_grid: OccupancyGrid, + clearance_m: float, + visited: NDArray[np.bool_] | None = None, +) -> tuple[float, float] | None: + clearance_cells = int(np.ceil(clearance_m / occupancy_grid.resolution)) + + free_mask = occupancy_grid.grid == 0 + if not np.any(free_mask): + return None + + # Erode the free mask by the clearance radius so only cells with full clearance remain. + structure = np.ones((2 * clearance_cells + 1, 2 * clearance_cells + 1), dtype=bool) + safe_mask = binary_erosion(free_mask, structure=structure) + + # Prefer unvisited cells; fall back to all safe cells if everything is visited. + if visited is not None: + unvisited_safe = safe_mask & ~visited + if np.any(unvisited_safe): + safe_mask = unvisited_safe + + safe_indices = np.argwhere(safe_mask) + if len(safe_indices) == 0: + return None + + idx = safe_indices[np.random.randint(len(safe_indices))] + row, col = idx + world = occupancy_grid.grid_to_world((col, row, 0)) + return (world.x, world.y) diff --git a/dimos/navigation/patrolling/routers/visitation_history.py b/dimos/navigation/patrolling/routers/visitation_history.py new file mode 100644 index 0000000000..fba50b4ceb --- /dev/null +++ b/dimos/navigation/patrolling/routers/visitation_history.py @@ -0,0 +1,116 @@ +# Copyright 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. + +import numpy as np +from numpy.typing import NDArray + +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid + + +def _circular_disk(radius_cells: int) -> NDArray[np.bool_]: + y, x = np.ogrid[-radius_cells : radius_cells + 1, -radius_cells : radius_cells + 1] + return np.asarray((x * x + y * y) <= radius_cells * radius_cells) + + +class VisitationHistory: + """Tracks visited locations in world coordinates, independent of occupancy grid changes. + + When a new occupancy grid arrives, the visited mask is rebuilt from stored + world-coordinate points. To avoid unbounded growth, when the visited + saturation reaches ``_saturation_threshold`` the oldest half of the stored + points are discarded and the mask is rebuilt. + """ + + _saturation_threshold = 0.50 + _min_distance_m = 0.05 + + def __init__(self, clearance_radius_m: float) -> None: + self._points: list[tuple[float, float]] = [] + self._visited: NDArray[np.bool_] | None = None + self._grid: OccupancyGrid | None = None + self._clearance_radius_m = clearance_radius_m + self._clearance_radius_cells: int = 0 + self._clearance_disk: NDArray[np.bool_] = np.ones((1, 1), dtype=bool) + + @property + def visited(self) -> NDArray[np.bool_] | None: + return self._visited + + @property + def clearance_radius_cells(self) -> int: + return self._clearance_radius_cells + + @property + def clearance_disk(self) -> NDArray[np.bool_]: + return self._clearance_disk + + def update_grid(self, grid: OccupancyGrid) -> None: + self._grid = grid + self._clearance_radius_cells = int(np.ceil(self._clearance_radius_m / grid.resolution)) + self._clearance_disk = _circular_disk(self._clearance_radius_cells) + self._rebuild() + + def handle_odom(self, x: float, y: float) -> None: + if self._points: + lx, ly = self._points[-1] + if (x - lx) ** 2 + (y - ly) ** 2 < self._min_distance_m**2: + return + self._points.append((x, y)) + if self._visited is None or self._grid is None: + return + self._stamp(x, y) + if self.get_saturation() >= self._saturation_threshold: + n = len(self._points) + self._points = self._points[n // 2 :] + self._rebuild() + + def get_saturation(self) -> float: + grid = self._grid + visited = self._visited + if grid is None or visited is None: + return 0.0 + free_mask = grid.grid == 0 + total = int(np.count_nonzero(free_mask)) + if total == 0: + return 0.0 + visited_free = int(np.count_nonzero(visited & free_mask)) + return visited_free / total + + def _rebuild(self) -> None: + grid = self._grid + if grid is None: + return + self._visited = np.zeros((grid.height, grid.width), dtype=bool) + for x, y in self._points: + self._stamp(x, y) + + def _stamp(self, x: float, y: float) -> None: + grid = self._grid + visited = self._visited + if grid is None or visited is None: + return + r = self._clearance_radius_cells + grid_pos = grid.world_to_grid((x, y)) + col, row = int(grid_pos.x), int(grid_pos.y) + if row + r < 0 or row - r >= grid.height or col + r < 0 or col - r >= grid.width: + return + r_min = max(0, row - r) + r_max = min(grid.height, row + r + 1) + c_min = max(0, col - r) + c_max = min(grid.width, col + r + 1) + d_r_min = r_min - (row - r) + d_r_max = d_r_min + (r_max - r_min) + d_c_min = c_min - (col - r) + d_c_max = d_c_min + (c_max - c_min) + visited[r_min:r_max, c_min:c_max] |= self._clearance_disk[d_r_min:d_r_max, d_c_min:d_c_max] diff --git a/dimos/navigation/patrolling/test_create_patrol_router.py b/dimos/navigation/patrolling/test_create_patrol_router.py new file mode 100644 index 0000000000..51e513c8b6 --- /dev/null +++ b/dimos/navigation/patrolling/test_create_patrol_router.py @@ -0,0 +1,101 @@ +# Copyright 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. + + +import os + +import cv2 +import numpy as np +import pytest + +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.mapping.occupancy.visualizations import visualize_occupancy_grid +from dimos.mapping.pointclouds.occupancy import height_cost_occupancy +from dimos.mapping.pointclouds.util import read_pointcloud +from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.utilities import point_to_pose_stamped +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.utils.data import get_data + + +@pytest.fixture +def big_office() -> OccupancyGrid: + data = read_pointcloud(get_data("big_office.ply")) + cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + return height_cost_occupancy(cloud) + + +@pytest.mark.slow +@pytest.mark.parametrize( + "router_name, saturation", [("random", 0.20), ("coverage", 0.30), ("frontier", 0.20)] +) +def test_patrolling_coverage(router_name, saturation, big_office) -> None: + start = (-1.03, -13.48) + robot_width = 0.4 + multiplier = 1.5 + big_office_gradient = gradient(big_office, max_distance=1.5) + router = create_patrol_router(router_name, robot_width * multiplier) + router.handle_occupancy_grid(big_office) + router.handle_odom(point_to_pose_stamped(start)) + + all_poses: list = [] + for _ in range(15): + goal = router.next_goal() + if goal is None: + continue + path = min_cost_astar( + big_office_gradient, goal.position, start, unknown_penalty=1.0, use_cpp=True + ) + if path is None: + continue + path = smooth_resample_path(path, goal, 0.1) + for pose in path.poses: + router.handle_odom(pose) + all_poses.append(pose) + start = (path.poses[-1].position.x, path.poses[-1].position.y) + + assert router.get_saturation() > saturation + + if os.environ.get("DEBUG"): + _save_coverage_image(router_name, router, all_poses, big_office, big_office_gradient) + + +def _save_coverage_image(router_name, router, all_poses, big_office, big_office_gradient) -> None: + image = visualize_occupancy_grid(big_office_gradient, "rainbow") + h, w = image.data.shape[:2] + visit_counts = np.zeros((h, w), dtype=np.float32) + radius = int(np.ceil(router._clearance_radius_m / big_office.resolution)) + stamp = np.zeros((h, w), dtype=np.uint8) + + for pose in all_poses: + grid = big_office.world_to_grid((pose.position.x, pose.position.y)) + gx, gy = int(grid.x), int(grid.y) + if 0 <= gy < h and 0 <= gx < w: + stamp[:] = 0 + cv2.circle(stamp, (gx, gy), radius, 1, -1) + visit_counts += stamp + + alpha = 0.05 + mask = visit_counts > 0 + blend = 1.0 - (1.0 - alpha) ** visit_counts + + overlay = image.data.astype(np.float32) * 0.24 + for c in range(3): + overlay[:, :, c][mask] = overlay[:, :, c][mask] * (1.0 - blend[mask]) + 255.0 * blend[mask] + + image.data = overlay.astype(np.uint8) + image.save(f"patrolling_coverage_{router_name}.png") diff --git a/dimos/navigation/patrolling/utilities.py b/dimos/navigation/patrolling/utilities.py new file mode 100644 index 0000000000..052cc29b91 --- /dev/null +++ b/dimos/navigation/patrolling/utilities.py @@ -0,0 +1,27 @@ +# Copyright 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 dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + +def point_to_pose_stamped(point: tuple[float, float]) -> PoseStamped: + pose = PoseStamped() + pose.position.x = point[0] + pose.position.y = point[1] + return pose + + +def pose_stamped_to_point(pose: PoseStamped) -> tuple[float, float]: + return (pose.position.x, pose.position.y) diff --git a/dimos/navigation/replanning_a_star/controllers.py b/dimos/navigation/replanning_a_star/controllers.py index 865aafb8be..9e8d570d36 100644 --- a/dimos/navigation/replanning_a_star/controllers.py +++ b/dimos/navigation/replanning_a_star/controllers.py @@ -103,12 +103,12 @@ def _apply_min_velocity(self, velocity: float, min_velocity: float) -> float: return velocity def _angular_twist(self, angular_velocity: float) -> Twist: - # In simulation, add a small forward velocity to help the locomotion - # policy execute rotation (some policies don't handle pure in-place rotation). - linear_x = 0.18 if self._global_config.simulation else 0.0 + # In simulation, we need stroger values + if self._global_config.simulation and abs(angular_velocity) < 0.8: + angular_velocity = 0.8 * np.sign(angular_velocity) return Twist( - linear=Vector3(linear_x, 0.0, 0.0), + linear=Vector3(0.0, 0.0, 0.0), angular=Vector3(0.0, 0.0, angular_velocity), ) diff --git a/dimos/navigation/replanning_a_star/global_planner.py b/dimos/navigation/replanning_a_star/global_planner.py index df2680a4a7..5158dcff08 100644 --- a/dimos/navigation/replanning_a_star/global_planner.py +++ b/dimos/navigation/replanning_a_star/global_planner.py @@ -52,6 +52,7 @@ class GlobalPlanner(Resource): _global_config: GlobalConfig _navigation_map: NavigationMap + _navigation_map_near: NavigationMap _local_planner: LocalPlanner _position_tracker: PositionTracker _replan_limiter: ReplanLimiter @@ -60,31 +61,40 @@ class GlobalPlanner(Resource): _replan_event: Event _replan_reason: StopMessage | None _lock: RLock + _safe_goal_clearance: float _safe_goal_tolerance: float = 4.0 _goal_tolerance: float = 0.2 _rotation_tolerance: float = math.radians(15) _replan_goal_tolerance: float = 0.5 - _max_replan_attempts: int = 10 _stuck_time_window: float = 8.0 + _stuck_threshold: float = 0.4 _max_path_deviation: float = 0.9 + _replanning_enabled: bool = True def __init__(self, global_config: GlobalConfig) -> None: self.path = Subject() self.goal_reached = Subject() self._global_config = global_config - self._navigation_map = NavigationMap(self._global_config) + self._navigation_map = NavigationMap(self._global_config, "voronoi") + self._navigation_map_near = NavigationMap(self._global_config, "gradient") self._local_planner = LocalPlanner( self._global_config, self._navigation_map, self._goal_tolerance ) - self._position_tracker = PositionTracker(self._stuck_time_window) + + stuck_threshold = self._stuck_threshold + if global_config.simulation: + stuck_threshold = 1.0 + + self._position_tracker = PositionTracker(self._stuck_time_window, stuck_threshold) self._replan_limiter = ReplanLimiter() self._disposables = CompositeDisposable() self._stop_planner = Event() self._replan_event = Event() self._replan_reason = None self._lock = RLock() + self._reset_safe_goal_clearance() def start(self) -> None: self._local_planner.start() @@ -117,6 +127,7 @@ def handle_odom(self, msg: PoseStamped) -> None: def handle_global_costmap(self, msg: OccupancyGrid) -> None: self._navigation_map.update(msg) + self._navigation_map_near.update(msg) def handle_goal_request(self, goal: PoseStamped) -> None: logger.info("Got new goal", goal=str(goal)) @@ -126,6 +137,13 @@ def handle_goal_request(self, goal: PoseStamped) -> None: self._replan_limiter.reset() self._plan_path() + def set_safe_goal_clearance(self, clearance: float) -> None: + with self._lock: + self._safe_goal_clearance = clearance + + def reset_safe_goal_clearance(self) -> None: + self._reset_safe_goal_clearance() + def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False) -> None: logger.info("Cancelling goal.", but_will_try_again=but_will_try_again, arrived=arrived) @@ -143,6 +161,10 @@ def cancel_goal(self, *, but_will_try_again: bool = False, arrived: bool = False if not but_will_try_again: self.goal_reached.on_next(Bool(arrived)) + def set_replanning_enabled(self, enabled: bool) -> None: + with self._lock: + self._replanning_enabled = enabled + def get_state(self) -> NavigationState: return self._local_planner.get_state() @@ -267,6 +289,10 @@ def _replan_path(self) -> None: self.cancel_goal(arrived=True) return + if not self._replanning_enabled: + self.cancel_goal() + return + if not self._replan_limiter.can_retry(current_odom.position): self.cancel_goal() return @@ -291,6 +317,10 @@ def _plan_path(self) -> None: safe_goal = self._find_safe_goal(current_goal.position) if not safe_goal: + logger.warning( + "No safe goal found.", x=round(current_goal.x, 3), y=round(current_goal.y, 3) + ) + self.cancel_goal() return path = self._find_wide_path(safe_goal, current_odom.position) @@ -299,6 +329,7 @@ def _plan_path(self) -> None: logger.warning( "No path found to the goal.", x=round(safe_goal.x, 3), y=round(safe_goal.y, 3) ) + self.cancel_goal() return resampled_path = smooth_resample_path(path, current_goal, 0.1) @@ -312,7 +343,9 @@ def _find_wide_path(self, goal: Vector3, robot_pos: Vector3) -> Path | None: sizes_to_try: list[float] = [1.1] for size in sizes_to_try: - costmap = self._navigation_map.make_gradient_costmap(size) + distance = robot_pos.distance(goal) + navigation_map = self._navigation_map if distance > 1.5 else self._navigation_map_near + costmap = navigation_map.make_gradient_costmap(size) path = min_cost_astar(costmap, goal, robot_pos) if path and path.poses: logger.info(f"Found path {size}x robot width.") @@ -331,7 +364,7 @@ def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: goal, algorithm="bfs_contiguous", cost_threshold=CostValues.OCCUPIED, - min_clearance=self._global_config.robot_rotation_diameter / 2, + min_clearance=self._safe_goal_clearance, max_search_distance=self._safe_goal_tolerance, ) @@ -346,3 +379,7 @@ def _find_safe_goal(self, goal: Vector3) -> Vector3 | None: logger.info("Found safe goal.", x=round(safe_goal.x, 2), y=round(safe_goal.y, 2)) return safe_goal + + def _reset_safe_goal_clearance(self) -> None: + with self._lock: + self._safe_goal_clearance = self._global_config.robot_rotation_diameter / 2 diff --git a/dimos/navigation/replanning_a_star/local_planner.py b/dimos/navigation/replanning_a_star/local_planner.py index a5f8d9e457..c61b991d97 100644 --- a/dimos/navigation/replanning_a_star/local_planner.py +++ b/dimos/navigation/replanning_a_star/local_planner.py @@ -85,9 +85,13 @@ def __init__( self._navigation_map = navigation_map self._goal_tolerance = goal_tolerance + speed = self._speed + if global_config.nerf_speed < 1.0: + speed *= global_config.nerf_speed + self._controller = PController( self._global_config, - self._speed, + speed, self._control_frequency, ) diff --git a/dimos/navigation/replanning_a_star/min_cost_astar.py b/dimos/navigation/replanning_a_star/min_cost_astar.py index c3430e64d9..3ac10a5f27 100644 --- a/dimos/navigation/replanning_a_star/min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/min_cost_astar.py @@ -195,8 +195,9 @@ def min_cost_astar( continue if neighbor_val == CostValues.UNKNOWN: - # Unknown cells have a moderate traversal cost cell_cost = cost_threshold * unknown_penalty + if cell_cost >= cost_threshold: + continue elif neighbor_val == CostValues.FREE: cell_cost = 0.0 else: diff --git a/dimos/navigation/replanning_a_star/min_cost_astar_cpp.cpp b/dimos/navigation/replanning_a_star/min_cost_astar_cpp.cpp index f19b3bf826..5b4a575197 100644 --- a/dimos/navigation/replanning_a_star/min_cost_astar_cpp.cpp +++ b/dimos/navigation/replanning_a_star/min_cost_astar_cpp.cpp @@ -201,8 +201,10 @@ std::vector> min_cost_astar_cpp( double cell_cost; if (val == COST_UNKNOWN) { - // Unknown cells have a moderate traversal cost cell_cost = cost_threshold * unknown_penalty; + if (cell_cost >= cost_threshold) { + continue; + } } else if (val == COST_FREE) { cell_cost = 0.0; } else { diff --git a/dimos/navigation/replanning_a_star/module.py b/dimos/navigation/replanning_a_star/module.py index 4dad9a2843..a96fd9a377 100644 --- a/dimos/navigation/replanning_a_star/module.py +++ b/dimos/navigation/replanning_a_star/module.py @@ -107,6 +107,18 @@ def cancel_goal(self) -> bool: self._planner.cancel_goal() return True + @rpc + def set_replanning_enabled(self, enabled: bool) -> None: + self._planner.set_replanning_enabled(enabled) + + @rpc + def set_safe_goal_clearance(self, clearance: float) -> None: + self._planner.set_safe_goal_clearance(clearance) + + @rpc + def reset_safe_goal_clearance(self) -> None: + self._planner.reset_safe_goal_clearance() + replanning_a_star_planner = ReplanningAStarPlanner.blueprint diff --git a/dimos/navigation/replanning_a_star/module_spec.py b/dimos/navigation/replanning_a_star/module_spec.py new file mode 100644 index 0000000000..32645059dc --- /dev/null +++ b/dimos/navigation/replanning_a_star/module_spec.py @@ -0,0 +1,29 @@ +# Copyright 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 typing import Protocol + +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.navigation.base import NavigationState +from dimos.spec.utils import Spec + + +class ReplanningAStarPlannerSpec(Spec, Protocol): + def set_goal(self, goal: PoseStamped) -> bool: ... + def get_state(self) -> NavigationState: ... + def is_goal_reached(self) -> bool: ... + def cancel_goal(self) -> bool: ... + def set_replanning_enabled(self, enabled: bool) -> None: ... + def set_safe_goal_clearance(self, clearance: float) -> None: ... + def reset_safe_goal_clearance(self) -> None: ... diff --git a/dimos/navigation/replanning_a_star/navigation_map.py b/dimos/navigation/replanning_a_star/navigation_map.py index f1c149ded6..fde75c0b0e 100644 --- a/dimos/navigation/replanning_a_star/navigation_map.py +++ b/dimos/navigation/replanning_a_star/navigation_map.py @@ -15,17 +15,20 @@ from threading import RLock from dimos.core.global_config import GlobalConfig +from dimos.mapping.occupancy.gradient import GradientStrategy from dimos.mapping.occupancy.path_map import make_navigation_map from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid class NavigationMap: _global_config: GlobalConfig + _gradient_strategy: GradientStrategy _binary: OccupancyGrid | None = None _lock: RLock - def __init__(self, global_config: GlobalConfig) -> None: + def __init__(self, global_config: GlobalConfig, gradient_strategy: GradientStrategy) -> None: self._global_config = global_config + self._gradient_strategy = gradient_strategy self._lock = RLock() def update(self, occupancy_grid: OccupancyGrid) -> None: @@ -62,5 +65,6 @@ def make_gradient_costmap(self, robot_increase: float = 1.0) -> OccupancyGrid: return make_navigation_map( binary, self._global_config.robot_width * robot_increase, - strategy=self._global_config.planner_strategy, + strategy="simple", + gradient_strategy=self._gradient_strategy, ) diff --git a/dimos/navigation/replanning_a_star/position_tracker.py b/dimos/navigation/replanning_a_star/position_tracker.py index 77b4df0dd0..7d8249b562 100644 --- a/dimos/navigation/replanning_a_star/position_tracker.py +++ b/dimos/navigation/replanning_a_star/position_tracker.py @@ -34,10 +34,10 @@ class PositionTracker: _index: int _size: int - def __init__(self, time_window: float) -> None: + def __init__(self, time_window: float, threshold: float) -> None: self._lock = RLock() self._time_window = time_window - self._threshold = 0.4 + self._threshold = threshold self._max_points = int(_max_points_per_second * self._time_window) self.reset_data() diff --git a/dimos/navigation/replanning_a_star/test_min_cost_astar.py b/dimos/navigation/replanning_a_star/test_min_cost_astar.py index 9cc0cad29a..1ea28d1cae 100644 --- a/dimos/navigation/replanning_a_star/test_min_cost_astar.py +++ b/dimos/navigation/replanning_a_star/test_min_cost_astar.py @@ -59,6 +59,54 @@ def test_astar_corner(costmap_three_paths) -> None: np.testing.assert_array_equal(actual.data, expected.data) +def test_astar_unknown_penalty_blocks_unknown_cells(costmap) -> None: + """With unknown_penalty=1.0, unknown cells should be untraversable.""" + # Create a grid with a corridor of free cells and unknown cells surrounding it. + # Place start and goal such that the shortest path would go through unknown cells + # but with penalty=1.0 it should either avoid them or return None. + grid = np.full((100, 100), -1, dtype=np.int8) # All unknown + # Carve a U-shaped free corridor: left column, bottom row, right column + grid[10:90, 10] = 0 # left column + grid[89, 10:90] = 0 # bottom row + grid[10:90, 89] = 0 # right column + og = OccupancyGrid(grid, resolution=0.1) + + start = og.grid_to_world((10, 10)) + goal = og.grid_to_world((89, 10)) + + for use_cpp in [False, True]: + path = min_cost_astar(og, goal, start, unknown_penalty=1.0, use_cpp=use_cpp) + if path is None: + # No path through unknown is also acceptable + continue + # Verify no path cell lands on an unknown cell + for pose in path.poses: + gp = og.world_to_grid((pose.position.x, pose.position.y)) + gx, gy = round(gp.x), round(gp.y) + if 0 <= gx < 100 and 0 <= gy < 100: + assert grid[gy, gx] != -1, ( + f"Path traverses unknown cell at grid ({gx}, {gy}), use_cpp={use_cpp}" + ) + + +def test_astar_unknown_penalty_allows_with_low_penalty(costmap) -> None: + """With unknown_penalty < 1.0, unknown cells should be traversable.""" + grid = np.full((50, 50), -1, dtype=np.int8) # All unknown + grid[5, 5] = 0 # start cell free + grid[45, 45] = 0 # goal cell free + og = OccupancyGrid(grid, resolution=0.1) + + start = og.grid_to_world((5, 5)) + goal = og.grid_to_world((45, 45)) + + for use_cpp in [False, True]: + path = min_cost_astar(og, goal, start, unknown_penalty=0.5, use_cpp=use_cpp) + assert path is not None, ( + f"Should find path through unknown with penalty=0.5, use_cpp={use_cpp}" + ) + assert len(path.poses) > 0 + + def test_astar_python_and_cpp(costmap) -> None: start = Vector3(4.0, 2.0, 0) goal = Vector3(6.15, 10.0) diff --git a/dimos/perception/perceive_loop_skill.py b/dimos/perception/perceive_loop_skill.py index 53362977f5..3e7a8793ef 100644 --- a/dimos/perception/perceive_loop_skill.py +++ b/dimos/perception/perceive_loop_skill.py @@ -16,7 +16,7 @@ import json from threading import RLock -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any from langchain_core.messages import HumanMessage @@ -44,7 +44,7 @@ class PerceiveLoopSkill(Module): color_image: In[Image] _agent_spec: AgentSpec - _period: float = 0.5 # seconds - how often to run the perceive loop + _period: float = 0.1 # seconds - how often to run the perceive loop def __init__( self, @@ -54,6 +54,7 @@ def __init__( self._global_config: GlobalConfig = cfg self._vl_model: VlModel = create(cfg.detection_model) self._active_lookout: tuple[str, ...] = () + self._then: dict[str, Any] | None = None self._lookout_subscription: DisposableBase | None = None self._model_started: bool = False self._lock = RLock() @@ -68,13 +69,40 @@ def stop(self) -> None: super().stop() @skill - def look_out_for(self, description_of_things: list[str]) -> str: + def look_out_for( + self, description_of_things: list[str], then: dict[str, Any] | None = None + ) -> str: """This tool will continuously look out for things matching the description in the input list, and notify the agent whenever it finds a match. After the match is found, it will stop. You can ask it for `look_out_for(["small dogs", "cats"])` and you will be notified back whenever such a detection is made. + + Optionally, you can specify a `then` parameter to automatically execute + another tool when a match is found, without waiting for the agent to + process the notification. This is useful for time-sensitive actions like + following a detected person. + + The `then` parameter is a dict with: + - "tool": name of the tool to call (e.g. "follow_person") + - "args": dict of arguments to pass to the tool + + In the args, you can use template variables that will be replaced with + detection data: + - "$bbox": the bounding box [x1, y1, x2, y2] of the best detection + - "$label": the label/name of the detection + - "$image": base64-encoded JPEG of the frame the detection was made on + + Example: + look_out_for(["person"], then={ + "tool": "follow_person", + "args": { + "query": "person", + "initial_bbox": "$bbox", + "initial_image": "$image", + } + }) """ with self._lock: @@ -90,6 +118,7 @@ def look_out_for(self, description_of_things: list[str]) -> str: self._vl_model.start() self._model_started = True self._active_lookout = tuple(description_of_things) + self._then = then self._lookout_subscription = sharpest.subscribe( on_next=self._on_image, on_error=lambda e: logger.exception("Error in perceive loop", exc_info=e), @@ -128,12 +157,30 @@ def _on_image(self, image: Image) -> None: self._lookout_subscription.dispose() self._lookout_subscription = None self._active_lookout = () + then = self._then + self._then = None self._vl_model.stop() self._model_started = False - self._agent_spec.add_message( - HumanMessage(f"Found a match for {active_lookout_str}. Please announce audibly.") + if then is None: + self._agent_spec.add_message( + HumanMessage(f"Found a match for {active_lookout_str}. Please announce audibly.") + ) + return + + best = max(detections.detections, key=lambda d: d.bbox_2d_volume()) + continuation_context: dict[str, Any] = { + "bbox": list(best.bbox), + "label": best.name, + "image": image.to_base64(quality=70), + } + logger.info( + "Lookout matched, dispatching continuation", + lookout=active_lookout_str, + continuation=then, + detection=continuation_context, ) + self._agent_spec.dispatch_continuation(then, continuation_context) def _stop_lookout(self) -> None: with self._lock: @@ -141,6 +188,7 @@ def _stop_lookout(self) -> None: self._lookout_subscription.dispose() self._lookout_subscription = None self._active_lookout = () + self._then = None if self._model_started: self._vl_model.stop() self._model_started = False diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py index 22743ac135..feadf22204 100644 --- a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2.py @@ -17,6 +17,7 @@ from dimos.mapping.costmapper import cost_mapper from dimos.mapping.voxels import voxel_mapper from dimos.navigation.frontier_exploration import wavefront_frontier_explorer +from dimos.navigation.patrolling.module import PatrollingModule from dimos.navigation.replanning_a_star.module import replanning_a_star_planner from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic @@ -26,6 +27,7 @@ cost_mapper(), replanning_a_star_planner(), wavefront_frontier_explorer(), + PatrollingModule.blueprint(), ).global_config(n_workers=7, robot_model="unitree_go2") __all__ = ["unitree_go2"] diff --git a/dimos/simulation/mujoco/direct_cmd_vel_explorer.py b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py new file mode 100644 index 0000000000..2654b9a554 --- /dev/null +++ b/dimos/simulation/mujoco/direct_cmd_vel_explorer.py @@ -0,0 +1,107 @@ +# Copyright 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. + +import math +import threading +from typing import TYPE_CHECKING + +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 + +if TYPE_CHECKING: + from collections.abc import Callable + + +class DirectCmdVelExplorer: + def __init__( + self, + linear_speed: float = 1.0, + rotation_speed: float = 1.5, + publish_rate: float = 10.0, + ) -> None: + self.linear_speed = linear_speed + self.rotation_speed = rotation_speed + self._dt = 1.0 / publish_rate + self._cmd_vel: LCMTransport[Twist] | None = None + self._odom: LCMTransport[PoseStamped] | None = None + self._pose: PoseStamped | None = None + self._new_pose = threading.Event() + self._unsub: Callable[[], None] | None = None + + def start(self) -> None: + self._cmd_vel = LCMTransport("/cmd_vel", Twist) + self._odom = LCMTransport("/odom", PoseStamped) + self._pose = None + self._unsub = self._odom.subscribe(self._on_odom) # type: ignore[func-returns-value] + + def stop(self) -> None: + if self._unsub: + self._unsub() + if self._cmd_vel: + self._cmd_vel.stop() + if self._odom: + self._odom.stop() + + def _on_odom(self, msg: PoseStamped) -> None: + self._pose = msg + self._new_pose.set() + + def _wait_for_pose(self) -> PoseStamped: + self._new_pose.clear() + self._new_pose.wait(timeout=5.0) + assert self._pose is not None, "No odom received" + return self._pose + + @staticmethod + def _normalize_angle(angle: float) -> float: + while angle > math.pi: + angle -= 2 * math.pi + while angle < -math.pi: + angle += 2 * math.pi + return angle + + def _stop(self) -> None: + assert self._cmd_vel is not None + self._cmd_vel.broadcast(None, Twist(linear=Vector3(), angular=Vector3())) + + def _drive_to(self, target_x: float, target_y: float) -> None: + """Pursuit controller: steer toward the target while driving forward.""" + while True: + pose = self._wait_for_pose() + dx = target_x - pose.x + dy = target_y - pose.y + distance = math.hypot(dx, dy) + if distance < 0.3: + break + target_heading = math.atan2(dy, dx) + heading_error = self._normalize_angle(target_heading - pose.yaw) + # Only drive forward when roughly facing the target. + if abs(heading_error) > 0.3: + linear = 0.0 + else: + linear = self.linear_speed + angular = max(-self.rotation_speed, min(self.rotation_speed, heading_error * 2.0)) + assert self._cmd_vel is not None + self._cmd_vel.broadcast( + None, + Twist(linear=Vector3(linear, 0, 0), angular=Vector3(0, 0, angular)), + ) + self._stop() + + def follow_points(self, waypoints: list[tuple[float, float]]) -> None: + self._wait_for_pose() + for tx, ty in waypoints: + self._drive_to(tx, ty) diff --git a/misc/optimize_patrol/optimize_candidates.py b/misc/optimize_patrol/optimize_candidates.py new file mode 100644 index 0000000000..f757e8fd4f --- /dev/null +++ b/misc/optimize_patrol/optimize_candidates.py @@ -0,0 +1,127 @@ +# Copyright 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. + +"""Find the optimal _candidates_to_consider value for CoveragePatrolRouter. + +For each candidate count, runs until TARGET_COVERAGE is reached and measures: + - Average next_goal() call duration (the planning cost) + - Distance traveled to reach the target coverage (path quality) + +Produces a single dual-axis chart with both metrics. +""" + +from concurrent.futures import ProcessPoolExecutor, as_completed +import json +import subprocess +import sys + +import matplotlib.pyplot as plt +import numpy as np + +CANDIDATE_VALUES = list(range(1, 16)) +N_ITERATIONS = 9 +TARGET_COVERAGE = 0.25 +MAX_WORKERS = 32 + + +def run_child(candidates: int) -> tuple[int, float, float]: + result = subprocess.run( + [ + sys.executable, + "-m", + "misc.optimize_patrol.optimize_candidates_child", + "--candidates", + str(candidates), + "--target_coverage", + str(TARGET_COVERAGE), + "--n_iterations", + str(N_ITERATIONS), + ], + capture_output=True, + text=True, + ) + if result.returncode != 0: + print(f"FAILED candidates={candidates}", file=sys.stderr) + print(result.stderr, file=sys.stderr) + return (candidates, float("nan"), float("nan")) + data = json.loads(result.stdout.strip()) + return (candidates, data["avg_next_goal_time"], data["distance"]) + + +def main() -> None: + print(f"Sweeping candidates_to_consider in {CANDIDATE_VALUES}") + print( + f" {N_ITERATIONS} iterations each, target coverage={TARGET_COVERAGE}, up to {MAX_WORKERS} workers" + ) + + results: dict[int, tuple[float, float]] = {} + + with ProcessPoolExecutor(max_workers=MAX_WORKERS) as pool: + futures = {pool.submit(run_child, c): c for c in CANDIDATE_VALUES} + for i, future in enumerate(as_completed(futures), 1): + cand, avg_time, distance = future.result() + results[cand] = (avg_time, distance) + print( + f"[{i}/{len(CANDIDATE_VALUES)}] candidates={cand}" + f" -> avg_next_goal={avg_time * 1000:.1f}ms distance={distance:.0f}m" + ) + + xs = sorted(results.keys()) + avg_times_ms = np.array([results[x][0] * 1000 for x in xs]) # Convert to ms. + distances = np.array([results[x][1] for x in xs]) + + # --- Single dual-axis chart --- + fig, ax1 = plt.subplots(figsize=(9, 5)) + color_time = "#FF5722" + color_dist = "#2196F3" + + ax1.set_xlabel("candidates_to_consider") + ax1.set_ylabel("Avg next_goal() duration (ms)", color=color_time) + ax1.plot( + xs, + avg_times_ms, + "s-", + color=color_time, + linewidth=2, + markersize=6, + label="Avg planning time", + ) + ax1.tick_params(axis="y", labelcolor=color_time) + ax1.set_xticks(xs) + + ax2 = ax1.twinx() + ax2.set_ylabel("Distance to reach target (m)", color=color_dist) + ax2.plot(xs, distances, "o-", color=color_dist, linewidth=2, markersize=6, label="Distance") + ax2.tick_params(axis="y", labelcolor=color_dist) + + fig.suptitle( + f"Planning cost vs path quality to reach {TARGET_COVERAGE:.0%} coverage" + f" (median of {N_ITERATIONS} iters)" + ) + fig.tight_layout() + out = "candidates_optimization.png" + fig.savefig(out, dpi=150, bbox_inches="tight") + print(f"\nChart saved to {out}") + plt.close(fig) + + # Summary table. + print("\n--- Summary ---") + print(f"{'candidates':>12} {'avg_time(ms)':>14} {'distance(m)':>14}") + for x in xs: + avg_t, dist = results[x] + print(f"{x:>12} {avg_t * 1000:>14.1f} {dist:>14.0f}") + + +if __name__ == "__main__": + main() diff --git a/misc/optimize_patrol/optimize_candidates_child.py b/misc/optimize_patrol/optimize_candidates_child.py new file mode 100644 index 0000000000..7a05e5a607 --- /dev/null +++ b/misc/optimize_patrol/optimize_candidates_child.py @@ -0,0 +1,173 @@ +# Copyright 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. + +"""Child process: test a single _candidates_to_consider value. + +Runs until target coverage is reached. Outputs JSON: + {"avg_next_goal_time": float, "distance": float} +""" + +import argparse +import json +import math +import time + +import numpy as np + +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.mapping.pointclouds.occupancy import height_cost_occupancy +from dimos.mapping.pointclouds.util import read_pointcloud +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.routers.coverage_patrol_router import CoveragePatrolRouter +from dimos.navigation.patrolling.utilities import point_to_pose_stamped +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.utils.data import get_data + +SCORING_STAMP_RADIUS_M = 0.2 +CLEARANCE_RADIUS_M = 0.2 +MAX_DISTANCE = 50_000.0 # Safety cap to avoid infinite loops. + + +def _circular_disk(radius_cells: int) -> np.ndarray: + y, x = np.ogrid[-radius_cells : radius_cells + 1, -radius_cells : radius_cells + 1] + return (x * x + y * y) <= radius_cells * radius_cells + + +def _stamp_scoring_map( + visited: np.ndarray, x: float, y: float, occupancy_grid, radius_cells: int, disk: np.ndarray +) -> None: + grid_pos = occupancy_grid.world_to_grid((x, y)) + col, row = int(grid_pos.x), int(grid_pos.y) + h, w = visited.shape + r = radius_cells + if row + r < 0 or row - r >= h or col + r < 0 or col - r >= w: + return + r_min = max(0, row - r) + r_max = min(h, row + r + 1) + c_min = max(0, col - r) + c_max = min(w, col + r + 1) + d_r_min = r_min - (row - r) + d_r_max = d_r_min + (r_max - r_min) + d_c_min = c_min - (col - r) + d_c_max = d_c_min + (c_max - c_min) + visited[r_min:r_max, c_min:c_max] |= disk[d_r_min:d_r_max, d_c_min:d_c_max] + + +def run_iteration( + candidates_to_consider: int, + target_coverage: float, + occupancy_grid, + costmap, + scoring_radius_cells: int, + scoring_disk: np.ndarray, +) -> tuple[float, float]: + """Returns (avg_next_goal_time_seconds, distance_traveled).""" + start = (-1.03, -13.48) + + router = create_patrol_router("coverage", CLEARANCE_RADIUS_M) + assert isinstance(router, CoveragePatrolRouter) + router._candidates_to_consider = candidates_to_consider + router.handle_occupancy_grid(occupancy_grid) + router.handle_odom(point_to_pose_stamped(start)) + + h, w = occupancy_grid.height, occupancy_grid.width + scoring_visited = np.zeros((h, w), dtype=bool) + free_mask = occupancy_grid.grid == 0 + total_free = int(np.count_nonzero(free_mask)) + if total_free == 0: + return 0.0, 0.0 + + _stamp_scoring_map( + scoring_visited, start[0], start[1], occupancy_grid, scoring_radius_cells, scoring_disk + ) + + distance_walked = 0.0 + next_goal_times: list[float] = [] + + while distance_walked < MAX_DISTANCE: + t0 = time.perf_counter() + goal = router.next_goal() + next_goal_times.append(time.perf_counter() - t0) + + if goal is None: + break + path = min_cost_astar(costmap, goal.position, start, unknown_penalty=1.0, use_cpp=True) + if path is None: + continue + path = smooth_resample_path(path, goal, 0.1) + + for pose in path.poses: + dx = pose.position.x - start[0] + dy = pose.position.y - start[1] + distance_walked += math.sqrt(dx * dx + dy * dy) + start = (pose.position.x, pose.position.y) + + router.handle_odom(pose) + _stamp_scoring_map( + scoring_visited, + pose.position.x, + pose.position.y, + occupancy_grid, + scoring_radius_cells, + scoring_disk, + ) + + coverage = int(np.count_nonzero(scoring_visited & free_mask)) / total_free + if coverage >= target_coverage: + break + + avg_time = float(np.mean(next_goal_times)) if next_goal_times else 0.0 + return avg_time, distance_walked + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--candidates", type=int, required=True) + parser.add_argument("--target_coverage", type=float, default=0.3) + parser.add_argument("--n_iterations", type=int, default=3) + args = parser.parse_args() + + data = read_pointcloud(get_data("big_office.ply")) + cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + occupancy_grid = height_cost_occupancy(cloud) + costmap = gradient(occupancy_grid, max_distance=1.5) + + scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.resolution)) + scoring_disk = _circular_disk(scoring_radius_cells) + + avg_times = [] + distances = [] + for _ in range(args.n_iterations): + avg_t, dist = run_iteration( + args.candidates, + args.target_coverage, + occupancy_grid, + costmap, + scoring_radius_cells, + scoring_disk, + ) + avg_times.append(avg_t) + distances.append(dist) + + result = { + "avg_next_goal_time": float(np.median(avg_times)), + "distance": float(np.median(distances)), + } + print(json.dumps(result)) + + +if __name__ == "__main__": + main() diff --git a/misc/optimize_patrol/optimize_patrol_router.py b/misc/optimize_patrol/optimize_patrol_router.py new file mode 100644 index 0000000000..1ebec949b8 --- /dev/null +++ b/misc/optimize_patrol/optimize_patrol_router.py @@ -0,0 +1,117 @@ +# Copyright 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. + +"""Parent process: matrix-test saturation_threshold and clearance_radius_m.""" + +from concurrent.futures import ProcessPoolExecutor, as_completed +import itertools +import subprocess +import sys + +import matplotlib.pyplot as plt +import numpy as np + +N_POINTS_SAT = 9 +N_POINTS_CLR = 10 +N_ITERATIONS = 5 +TOTAL_DISTANCE = 4000.0 +MAX_WORKERS = 32 + +SAT_MIN, SAT_MAX = 0.1, 0.9 +CLR_MIN, CLR_MAX = 0.1, 1.0 + + +def run_child(saturation_threshold: float, clearance_radius_m: float) -> tuple[float, float, float]: + result = subprocess.run( + [ + sys.executable, + "-m", + "misc.optimize_patrol.optimize_patrol_router_child", + "--saturation_threshold", + str(saturation_threshold), + "--clearance_radius_m", + str(clearance_radius_m), + "--n_iterations", + str(N_ITERATIONS), + "--total_distance", + str(TOTAL_DISTANCE), + ], + capture_output=True, + text=True, + ) + if result.returncode != 0: + print(f"FAILED sat={saturation_threshold} clr={clearance_radius_m}", file=sys.stderr) + print(result.stderr, file=sys.stderr) + return (saturation_threshold, clearance_radius_m, float("nan")) + score = float(result.stdout.strip()) + return (saturation_threshold, clearance_radius_m, score) + + +def main() -> None: + sat_values = np.linspace(SAT_MIN, SAT_MAX, N_POINTS_SAT) + clr_values = np.linspace(CLR_MIN, CLR_MAX, N_POINTS_CLR) + combos = list(itertools.product(sat_values, clr_values)) + + print(f"Running {len(combos)} combinations with up to {MAX_WORKERS} workers...") + + results: dict[tuple[float, float], float] = {} + + with ProcessPoolExecutor(max_workers=MAX_WORKERS) as pool: + futures = {pool.submit(run_child, sat, clr): (sat, clr) for sat, clr in combos} + for i, future in enumerate(as_completed(futures), 1): + sat, clr, score = future.result() + results[(sat, clr)] = score + print(f"[{i}/{len(combos)}] sat={sat:.3f} clr={clr:.3f} -> score={score:.4f}") + + # Build matrix for plotting. + matrix = np.zeros((N_POINTS_SAT, N_POINTS_CLR)) + for i, sat in enumerate(sat_values): + for j, clr in enumerate(clr_values): + matrix[i, j] = results.get((sat, clr), float("nan")) + + fig, ax = plt.subplots(figsize=(8, 6)) + im = ax.imshow(matrix, origin="lower", aspect="auto", cmap="viridis") + ax.set_xticks(range(N_POINTS_CLR)) + ax.set_xticklabels([f"{v:.2f}" for v in clr_values]) + ax.set_yticks(range(N_POINTS_SAT)) + ax.set_yticklabels([f"{v:.2f}" for v in sat_values]) + ax.set_xlabel("clearance_radius_m") + ax.set_ylabel("saturation_threshold") + ax.set_title(f"Coverage score (median of {N_ITERATIONS} iters, {TOTAL_DISTANCE}m walk)") + cbar = fig.colorbar(im, ax=ax) + cbar.set_label("Coverage (fraction of free cells visited)") + + # Annotate cells with values. + for i in range(N_POINTS_SAT): + for j in range(N_POINTS_CLR): + val = matrix[i, j] + if not np.isnan(val): + ax.text( + j, + i, + f"{val:.3f}", + ha="center", + va="center", + fontsize=8, + color="white" if val < (np.nanmax(matrix) + np.nanmin(matrix)) / 2 else "black", + ) + + out_path = "patrol_router_optimization.png" + fig.savefig(out_path, dpi=150, bbox_inches="tight") + print(f"Chart saved to {out_path}") + plt.close(fig) + + +if __name__ == "__main__": + main() diff --git a/misc/optimize_patrol/optimize_patrol_router_child.py b/misc/optimize_patrol/optimize_patrol_router_child.py new file mode 100644 index 0000000000..f6cb2b5365 --- /dev/null +++ b/misc/optimize_patrol/optimize_patrol_router_child.py @@ -0,0 +1,155 @@ +# Copyright 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. + +"""Child process: test a single (saturation_threshold, clearance_radius_m) pair.""" + +import argparse +import math + +import numpy as np + +from dimos.mapping.occupancy.gradient import gradient +from dimos.mapping.occupancy.path_resampling import smooth_resample_path +from dimos.mapping.pointclouds.occupancy import height_cost_occupancy +from dimos.mapping.pointclouds.util import read_pointcloud +from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.navigation.patrolling.create_patrol_router import create_patrol_router +from dimos.navigation.patrolling.routers.visitation_history import VisitationHistory +from dimos.navigation.patrolling.utilities import point_to_pose_stamped +from dimos.navigation.replanning_a_star.min_cost_astar import min_cost_astar +from dimos.utils.data import get_data + +SCORING_STAMP_RADIUS_M = 0.2 + + +def _circular_disk(radius_cells: int) -> np.ndarray: + y, x = np.ogrid[-radius_cells : radius_cells + 1, -radius_cells : radius_cells + 1] + return (x * x + y * y) <= radius_cells * radius_cells + + +def _stamp_scoring_map( + visited: np.ndarray, x: float, y: float, occupancy_grid, radius_cells: int, disk: np.ndarray +) -> None: + grid_pos = occupancy_grid.world_to_grid((x, y)) + col, row = int(grid_pos.x), int(grid_pos.y) + h, w = visited.shape + r = radius_cells + if row + r < 0 or row - r >= h or col + r < 0 or col - r >= w: + return + r_min = max(0, row - r) + r_max = min(h, row + r + 1) + c_min = max(0, col - r) + c_max = min(w, col + r + 1) + d_r_min = r_min - (row - r) + d_r_max = d_r_min + (r_max - r_min) + d_c_min = c_min - (col - r) + d_c_max = d_c_min + (c_max - c_min) + visited[r_min:r_max, c_min:c_max] |= disk[d_r_min:d_r_max, d_c_min:d_c_max] + + +def run_iteration( + saturation_threshold: float, + clearance_radius_m: float, + total_distance: float, + occupancy_grid, + costmap, + scoring_radius_cells: int, + scoring_disk: np.ndarray, +) -> float: + start = (-1.03, -13.48) + + VisitationHistory._saturation_threshold = saturation_threshold + router = create_patrol_router("coverage", clearance_radius_m) + router.handle_occupancy_grid(occupancy_grid) + router.handle_odom(point_to_pose_stamped(start)) + + h, w = occupancy_grid.height, occupancy_grid.width + scoring_visited = np.zeros((h, w), dtype=bool) + free_mask = occupancy_grid.grid == 0 + + _stamp_scoring_map( + scoring_visited, start[0], start[1], occupancy_grid, scoring_radius_cells, scoring_disk + ) + + distance_walked = 0.0 + + while distance_walked < total_distance: + goal = router.next_goal() + if goal is None: + break + path = min_cost_astar(costmap, goal.position, start, unknown_penalty=1.0, use_cpp=True) + if path is None: + continue + path = smooth_resample_path(path, goal, 0.1) + + for pose in path.poses: + dx = pose.position.x - start[0] + dy = pose.position.y - start[1] + distance_walked += math.sqrt(dx * dx + dy * dy) + start = (pose.position.x, pose.position.y) + + router.handle_odom(pose) + _stamp_scoring_map( + scoring_visited, + pose.position.x, + pose.position.y, + occupancy_grid, + scoring_radius_cells, + scoring_disk, + ) + + if distance_walked >= total_distance: + break + + total_free = int(np.count_nonzero(free_mask)) + if total_free == 0: + return 0.0 + return int(np.count_nonzero(scoring_visited & free_mask)) / total_free + + +def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument("--saturation_threshold", type=float, required=True) + parser.add_argument("--clearance_radius_m", type=float, required=True) + parser.add_argument("--n_iterations", type=int, default=5) + parser.add_argument("--total_distance", type=float, default=100.0) + args = parser.parse_args() + + data = read_pointcloud(get_data("big_office.ply")) + cloud = PointCloud2.from_numpy(np.asarray(data.points), frame_id="") + occupancy_grid = height_cost_occupancy(cloud) + costmap = gradient(occupancy_grid, max_distance=1.5) + + scoring_radius_cells = int(np.ceil(SCORING_STAMP_RADIUS_M / occupancy_grid.resolution)) + scoring_disk = _circular_disk(scoring_radius_cells) + + scores = [] + for _ in range(args.n_iterations): + score = run_iteration( + args.saturation_threshold, + args.clearance_radius_m, + args.total_distance, + occupancy_grid, + costmap, + scoring_radius_cells, + scoring_disk, + ) + scores.append(score) + + median = float(np.median(scores)) + print(median) + + +if __name__ == "__main__": + main() diff --git a/misc/optimize_patrol/plot_path.py b/misc/optimize_patrol/plot_path.py new file mode 100644 index 0000000000..7c7d2d4ad8 --- /dev/null +++ b/misc/optimize_patrol/plot_path.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +# Copyright 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. + +"""Record robot position from /odom and plot the travel path on Ctrl+C.""" + +from __future__ import annotations + +import argparse +import math +import signal +import time + +import matplotlib.pyplot as plt +import numpy as np + +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import PoseStamped + +MIN_DIST = 0.05 # minimum distance (m) between recorded points +STUCK_RADIUS = 0.6 # if robot stays within this radius (m) ... +STUCK_TIMEOUT = 60.0 # ... for this many seconds, stop recording + + +def main() -> None: + parser = argparse.ArgumentParser(description="Record /odom and plot patrol path") + parser.add_argument("--output", "-o", default="patrol_path.ignore.png") + args = parser.parse_args() + + transport: LCMTransport[PoseStamped] = LCMTransport("/odom", PoseStamped) + + xs: list[float] = [] + ys: list[float] = [] + t_start: list[float] = [] # single-element list so closure can mutate + stuck_anchor: list[float] = [0.0, 0.0] # (x, y) center for stuck detection + stuck_since: list[float] = [0.0] # timestamp when robot entered current stuck zone + stop = False + + def on_msg(msg: PoseStamped) -> None: + nonlocal stop + x, y = msg.position.x, msg.position.y + + # Record start time on first message. + if not t_start: + t_start.append(time.time()) + stuck_anchor[0], stuck_anchor[1] = x, y + stuck_since[0] = time.time() + + # Only record if far enough from the last recorded point. + if xs: + dx = x - xs[-1] + dy = y - ys[-1] + if math.hypot(dx, dy) < MIN_DIST: + return + + xs.append(x) + ys.append(y) + + # Stuck detection: check if robot left the stuck circle. + dist_from_anchor = math.hypot(x - stuck_anchor[0], y - stuck_anchor[1]) + if dist_from_anchor > STUCK_RADIUS: + # Robot moved out — reset anchor to current position. + stuck_anchor[0], stuck_anchor[1] = x, y + stuck_since[0] = time.time() + elif time.time() - stuck_since[0] > STUCK_TIMEOUT: + print( + f"\nRobot stuck within {STUCK_RADIUS}m radius for >{STUCK_TIMEOUT:.0f}s — stopping." + ) + stop = True + + transport.start() + transport.subscribe(on_msg) + + print("Listening on /odom ... recording positions. Press Ctrl+C to stop and plot.") + + def _handle_sigint(_sig: int, _frame: object) -> None: + nonlocal stop + stop = True + + signal.signal(signal.SIGINT, _handle_sigint) + + while not stop: + time.sleep(0.05) + + transport.stop() + t_end = time.time() + + # Compute stats. + elapsed = t_end - t_start[0] + mins, secs = divmod(elapsed, 60) + + xs_arr = np.array(xs) + ys_arr = np.array(ys) + dists = np.hypot(np.diff(xs_arr), np.diff(ys_arr)) + total_dist = float(np.sum(dists)) + + print(f"Recorded {len(xs)} points over {int(mins)}m{secs:.0f}s, {total_dist:.1f}m traveled.") + + fig, ax = plt.subplots(figsize=(10, 10)) + + for i in range(len(xs_arr) - 1): + ax.plot(xs_arr[i : i + 2], ys_arr[i : i + 2], color="blue", alpha=0.2, linewidth=2) + + ax.plot(xs_arr[0], ys_arr[0], "go", markersize=10, label="Start") + ax.plot(xs_arr[-1], ys_arr[-1], "ro", markersize=10, label="End") + + ax.set_xlabel("X (m)") + ax.set_ylabel("Y (m)") + ax.set_title(f"Patrol Path — {int(mins)}m{secs:.0f}s, {total_dist:.1f}m traveled") + ax.set_aspect("equal") + ax.legend() + ax.grid(True, alpha=0.3) + + fig.tight_layout() + fig.savefig(args.output, dpi=150) + print(f"Saved plot to {args.output}") + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index 03076f5ff4..79a12bd6dc 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -261,17 +261,17 @@ dev = [ "types-gevent>=25.4.0.20250915,<26", "types-greenlet>=3.2.0.20250915,<4", "types-jmespath>=1.0.2.20250809,<2", + "types-requests>=2.32.4.20260107,<3", "types-jsonschema>=4.25.1.20251009,<5", "types-networkx>=3.5.0.20251001,<4", "types-protobuf>=6.32.1.20250918,<7", - "types-psutil>=7.0.0.20251001,<8", + "types-psutil>=7.2.2.20260130,<8", + "types-psycopg2>=2.9.21.20251012", "types-pytz>=2025.2.0.20250809,<2026", "types-simplejson>=3.20.0.20250822,<4", "types-tabulate>=0.9.0.20241207,<1", "types-tensorflow>=2.18.0.20251008,<3", "types-tqdm>=4.67.0.20250809,<5", - "types-psycopg2>=2.9.21.20251012", - "types-psutil>=7.2.2.20260130,<8", # Tools "py-spy", @@ -393,6 +393,7 @@ module = [ "plotext", "plum.*", "portal", + "psutil", "pycuda", "pycuda.*", "pydrake", @@ -401,10 +402,13 @@ module = [ "pyzed.*", "rclpy.*", "sam2.*", + "scipy", + "scipy.*", "sensor_msgs.*", "std_msgs.*", "tf2_msgs.*", "torchreid", + "turbojpeg", "ultralytics.*", "unitree_webrtc_connect.*", "xarm.*", diff --git a/uv.lock b/uv.lock index 7e71e9637f..a57bccfc81 100644 --- a/uv.lock +++ b/uv.lock @@ -1805,6 +1805,7 @@ dds = [ { name = "types-pysocks" }, { name = "types-pytz" }, { name = "types-pyyaml" }, + { name = "types-requests" }, { name = "types-simplejson" }, { name = "types-tabulate" }, { name = "types-tensorflow" }, @@ -1842,6 +1843,7 @@ dev = [ { name = "types-pysocks" }, { name = "types-pytz" }, { name = "types-pyyaml" }, + { name = "types-requests" }, { name = "types-simplejson" }, { name = "types-tabulate" }, { name = "types-tensorflow" }, @@ -2119,12 +2121,12 @@ requires-dist = [ { name = "types-jsonschema", marker = "extra == 'dev'", specifier = ">=4.25.1.20251009,<5" }, { name = "types-networkx", marker = "extra == 'dev'", specifier = ">=3.5.0.20251001,<4" }, { name = "types-protobuf", marker = "extra == 'dev'", specifier = ">=6.32.1.20250918,<7" }, - { name = "types-psutil", marker = "extra == 'dev'", specifier = ">=7.0.0.20251001,<8" }, { name = "types-psutil", marker = "extra == 'dev'", specifier = ">=7.2.2.20260130,<8" }, { name = "types-psycopg2", marker = "extra == 'dev'", specifier = ">=2.9.21.20251012" }, { name = "types-pysocks", marker = "extra == 'dev'", specifier = ">=1.7.1.20251001,<2" }, { name = "types-pytz", marker = "extra == 'dev'", specifier = ">=2025.2.0.20250809,<2026" }, { name = "types-pyyaml", marker = "extra == 'dev'", specifier = ">=6.0.12.20250915,<7" }, + { name = "types-requests", marker = "extra == 'dev'", specifier = ">=2.32.4.20260107,<3" }, { name = "types-simplejson", marker = "extra == 'dev'", specifier = ">=3.20.0.20250822,<4" }, { name = "types-tabulate", marker = "extra == 'dev'", specifier = ">=0.9.0.20241207,<1" }, { name = "types-tensorflow", marker = "extra == 'dev'", specifier = ">=2.18.0.20251008,<3" },