From 700d18b4afff1a4e0dbfc968b21f4b31f15e33e5 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Mon, 31 Aug 2026 18:39:38 -0700 Subject: [PATCH] refactor(perception): replace OSR tracking with Dan localization --- dimos/manipulation/conftest.py | 3 +- dimos/manipulation/grasping/grasp_gen_spec.py | 9 - dimos/manipulation/grasping/grasping.py | 137 ---- .../manipulation/grasping/test_grasp_gen_x.py | 10 +- dimos/manipulation/manipulation_module.py | 14 - dimos/manipulation/pick_and_place_module.py | 65 +- .../planning/monitor/test_world_monitor.py | 106 --- .../planning/monitor/world_monitor.py | 48 -- .../monitor/world_obstacle_monitor.py | 249 +------ .../manipulation/test_manipulation_module.py | 1 - dimos/manipulation/test_manipulation_unit.py | 11 +- .../manipulation/test_pick_and_place_unit.py | 105 ++- .../demo_object_scene_registration.py | 38 -- dimos/perception/experimental/objectDB.py | 340 ---------- .../experimental/object_scene_registration.py | 633 +++++------------- .../object_scene_registration_spec.py | 14 +- .../test_object_scene_registration.py | 189 ++++++ ...test_object_scene_registration_temporal.py | 523 --------------- dimos/robot/all_blueprints.py | 2 - .../manipulators/common/agent_prompts.py | 12 +- .../xarm/blueprints/perception.py | 21 +- .../xarm/blueprints/simulation.py | 20 +- .../xarm/blueprints/test_perception.py | 62 ++ .../manipulation/xarm-room-sim.md | 19 +- 24 files changed, 585 insertions(+), 2046 deletions(-) delete mode 100644 dimos/manipulation/grasping/grasping.py delete mode 100644 dimos/perception/experimental/demo_object_scene_registration.py delete mode 100644 dimos/perception/experimental/objectDB.py create mode 100644 dimos/perception/experimental/test_object_scene_registration.py delete mode 100644 dimos/perception/experimental/test_object_scene_registration_temporal.py create mode 100644 dimos/robot/manipulators/xarm/blueprints/test_perception.py diff --git a/dimos/manipulation/conftest.py b/dimos/manipulation/conftest.py index 5bb4555d6d..f99e503303 100644 --- a/dimos/manipulation/conftest.py +++ b/dimos/manipulation/conftest.py @@ -77,9 +77,8 @@ def create(coordinator: ControlCoordinator | None = None) -> ManipulationModule: ) cast("Any", module).coordinator_joint_state = None # Nulling a port drops it from Module.inputs, so start() does not bind - # handle_voxel_map and no transport is needed. Same reason as above. + # handle_voxel_map and no transport is needed. cast("Any", module).voxel_map = None - cast("Any", module).objects = None mocker.patch.object(module, "_initialize_planning") module.start() return module diff --git a/dimos/manipulation/grasping/grasp_gen_spec.py b/dimos/manipulation/grasping/grasp_gen_spec.py index d3071d76d8..07e3882a9a 100644 --- a/dimos/manipulation/grasping/grasp_gen_spec.py +++ b/dimos/manipulation/grasping/grasp_gen_spec.py @@ -14,19 +14,10 @@ from typing import Protocol -from dimos.msgs.geometry_msgs.PoseArray import PoseArray from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.spec.utils import Spec -class LegacyGraspGenSpec(Spec, Protocol): - def generate_grasps( - self, - pointcloud: PointCloud2, - scene_pointcloud: PointCloud2 | None = None, - ) -> PoseArray | None: ... - - class GraspGenSpec(Spec, Protocol): def propose_grasps(self, object_pointcloud: PointCloud2) -> GraspCandidateArray: ... diff --git a/dimos/manipulation/grasping/grasping.py b/dimos/manipulation/grasping/grasping.py deleted file mode 100644 index 9fd1cc9178..0000000000 --- a/dimos/manipulation/grasping/grasping.py +++ /dev/null @@ -1,137 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Grasping skill module - -Provides @skill interface for agents and orchestrates the grasp generation pipeline: -perception (get pointcloud) to graspgen (generate grasps in Docker) to output grasps -""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from dimos.agents.annotation import skill -from dimos.core.core import rpc -from dimos.core.module import Module -from dimos.core.stream import Out -from dimos.manipulation.grasping.grasp_gen_spec import LegacyGraspGenSpec -from dimos.msgs.geometry_msgs.PoseArray import PoseArray -from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec -from dimos.utils.logging_config import setup_logger -from dimos.utils.transform_utils import quaternion_to_euler - -if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - -logger = setup_logger() - - -class GraspingModule(Module): - """Grasping skill and orchestrator module""" - - grasps: Out[PoseArray] - - _scene_registration: ObjectSceneRegistrationSpec - _grasp_gen: LegacyGraspGenSpec - - @rpc - def start(self) -> None: - super().start() - logger.info("GraspingModule started") - - @rpc - def stop(self) -> None: - super().stop() - logger.info("GraspingModule stopped") - - @skill - def generate_grasps( - self, - object_name: str = "object", - object_id: str | None = None, - filter_collisions: bool = True, - ) -> str: - """Generate grasp poses for the specified object. - - Args: - object_name: Name of the object to grasp (e.g. "coke can", "cup", "bottle"). - object_id: Optional unique object ID from perception. If provided, uses this - instead of object_name for lookup. - filter_collisions: Whether to filter grasps that collide with scene geometry. - - """ - # Get object pointcloud from perception - pc = self._get_object_pointcloud(object_name, object_id) - if pc is None: - msg = f"No pointcloud found for '{object_id or object_name}'" - logger.warning(msg) - return msg - - # Get scene pointcloud for collision filtering - scene_pc = None - if filter_collisions: - scene_pc = self._get_scene_pointcloud(exclude_object_id=object_id) - - # Call GraspGenModule (running in Docker) - try: - result = self._grasp_gen.generate_grasps(pc, scene_pc) - except Exception as e: - msg = f"Grasp generation failed: {e}" - logger.error(msg) - return msg - - if result is None or len(result.poses) == 0: - msg = f"No grasps generated for '{object_name}'" - logger.info(msg) - return msg - - self.grasps.publish(result) - logger.info(f"Generated {len(result.poses)} grasps for '{object_name}'") - - # Format result for agent/human - return self._format_grasp_result(result, object_name) - - def _get_object_pointcloud( - self, object_name: str, object_id: str | None = None - ) -> PointCloud2 | None: - """Fetch object pointcloud from perception.""" - try: - if object_id is not None: - return self._scene_registration.get_object_pointcloud_by_object_id(object_id) - - return self._scene_registration.get_object_pointcloud_by_name(object_name) - except Exception as e: - logger.error(f"Failed to get object pointcloud: {e}") - return None - - def _get_scene_pointcloud(self, exclude_object_id: str | None = None) -> PointCloud2 | None: - """Fetch scene pointcloud from perception for collision filtering.""" - try: - return self._scene_registration.get_full_scene_pointcloud( - exclude_object_id=exclude_object_id - ) - except Exception as e: - logger.debug(f"Could not get scene pointcloud: {e}") - return None - - def _format_grasp_result(self, grasps: PoseArray, object_name: str) -> str: - """Format grasp result for agent/human consumption.""" - best = grasps.poses[0] - pos = best.position - rpy = quaternion_to_euler(best.orientation, degrees=True) - return ( - f"Generated {len(grasps.poses)}" - f"Best grasp: pos=({pos.x:.4f}, {pos.y:.4f}, {pos.z:.4f}), " - f"rpy=({rpy.x:.1f}, {rpy.y:.1f}, {rpy.z:.1f}) degrees" - ) diff --git a/dimos/manipulation/grasping/test_grasp_gen_x.py b/dimos/manipulation/grasping/test_grasp_gen_x.py index 5621eeb711..175927ba23 100644 --- a/dimos/manipulation/grasping/test_grasp_gen_x.py +++ b/dimos/manipulation/grasping/test_grasp_gen_x.py @@ -25,7 +25,7 @@ import pytest from pytest_mock import MockerFixture -from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec, LegacyGraspGenSpec +from dimos.manipulation.grasping.grasp_gen_spec import GraspGenSpec import dimos.manipulation.grasping.grasp_gen_x as grasp_gen_x from dimos.manipulation.grasping.grasp_gen_x import ( GraspGenXConfig, @@ -107,15 +107,9 @@ def test_messages_round_trip_empty_and_score() -> None: ) -def test_ranked_spec_is_canonical_during_legacy_contract_transition() -> None: - legacy_signature = inspect.signature(LegacyGraspGenSpec.generate_grasps) +def test_ranked_spec_is_canonical() -> None: signature = inspect.signature(GraspGenSpec.propose_grasps) - assert list(legacy_signature.parameters) == [ - "self", - "pointcloud", - "scene_pointcloud", - ] assert list(signature.parameters) == ["self", "object_pointcloud"] assert signature.parameters["object_pointcloud"].annotation.__name__ == "PointCloud2" assert signature.return_annotation is GraspCandidateArray diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c8f96e8862..5deff3b20a 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -107,7 +107,6 @@ from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.perception.experimental.object import Object as DetObject from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -190,7 +189,6 @@ class ManipulationModule(Module): # Input: occupied cells of a mapped workspace, in the planning frame. Each # message is a complete map, so it replaces the obstacle rather than adding. voxel_map: In[PointCloud2] - objects: In[list[DetObject]] tf: Out[TFMessage] def __init__(self, **kwargs: Any) -> None: @@ -1319,18 +1317,6 @@ def world_monitor(self) -> WorldMonitor | None: """Access the world monitor for advanced obstacle/world operations.""" return self._world_monitor - async def handle_objects(self, objects: list[DetObject]) -> None: - """Cache the latest perception objects for an explicit obstacle refresh.""" - if self._world_monitor is not None: - self._world_monitor.on_objects(objects) - - @rpc - def refresh_obstacles(self, min_duration: float = 0.0) -> int: - """Sync cached perception objects into the planning world.""" - if self._world_monitor is None: - return 0 - return len(self._world_monitor.refresh_obstacles(min_duration)) - @rpc def get_obstacles(self) -> dict[str, PoseStamped]: """Return planning-world obstacle poses keyed by obstacle name.""" diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index 237310dff6..170c36be6e 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/dimos/manipulation/pick_and_place_module.py @@ -41,6 +41,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray from dimos.perception.experimental.object_scene_registration_spec import ObjectSceneRegistrationSpec +from dimos.perception.memory.types import Localization class PickAndPlaceModuleConfig(ModuleConfig): @@ -60,56 +61,62 @@ class PickAndPlaceModule(Module): def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._objects: dict[str, dict[str, Any]] = {} + self._objects: dict[int, dict[str, Any]] = {} + self._localizations: dict[int, Localization] = {} self._grasp_candidates = GraspCandidateArray() - self._selected_object_id: str | None = None + self._selected_object: int | None = None self._selected_grasp: PoseStamped | None = None self._holding_object = False @skill def scan_objects(self, prompts: list[str]) -> SkillResult[ManipulationSkillError]: - """Scan the latest RGB-D frame for prompted objects. + """Localize prompted objects from recent RGB-D history. Args: - prompts: Object labels to detect. Use an ID from this scan with pick_object. + prompts: Unique object labels to localize. Use a selection from this scan + with pick_object. """ prompts = [prompt.strip() for prompt in prompts if prompt.strip()] if not prompts: return SkillResult.fail("INVALID_INPUT", "At least one object prompt is required") + if len(set(prompts)) != len(prompts): + return SkillResult.fail("INVALID_INPUT", "Object prompts must be unique") if not self._holding_object: self._clear_selection() self._objects = {} + self._localizations = {} try: - detections = self._scene.scan_scene(text=prompts) - except RuntimeError as exc: + localizations = self._scene.localize_objects(prompts) + except (RuntimeError, ValueError) as exc: return SkillResult.fail("PERCEPTION_FAILED", str(exc)) - objects = [ - { - "object_id": str(detection.id), - "name": str(detection.results[0].hypothesis.class_id), + for prompt, localization in zip(prompts, localizations, strict=True): + if localization is None: + continue + selection = len(self._localizations) + self._localizations[selection] = localization + self._objects[selection] = { + "selection": selection, + "name": prompt, + "score": localization.semantic_score, } - for detection in detections.detections - if detection.id and detection.results - ] - self._objects = {str(obj["object_id"]): obj for obj in objects if "object_id" in obj} return SkillResult.ok( - f"Detected {detections.detections_length} object(s)", + f"Localized {len(self._localizations)} object(s)", prompts=prompts, objects=list(self._objects.values()), ) @rpc - def get_object(self, object_id: str) -> dict[str, Any] | None: - return self._objects.get(object_id) + def get_object(self, selection: int) -> dict[str, Any] | None: + return self._objects.get(selection) @skill(uses=[CAP_MOVEMENT]) def pick_object( - self, object_id: str, planning_group: PlanningGroupID | None = None + self, selection: int, planning_group: PlanningGroupID | None = None ) -> SkillResult[ManipulationSkillError]: - """Generate ranked grasps and pick one object from the latest scan. + """Generate ranked grasps and pick one localization from the latest scan. Args: - object_id: Exact object ID returned by the latest scan_objects call. + selection: Integer selection returned by the latest scan_objects call. planning_group: Gripper-capable pose group; omitted only when unambiguous. """ if self._holding_object: @@ -117,15 +124,15 @@ def pick_object( "INVALID_STATE", "Place the held object before starting another pick" ) self._clear_selection() - if object_id not in self._objects: - return SkillResult.fail("OBJECT_NOT_DETECTED", f"Unknown object_id: {object_id}") + localization = self._localizations.get(selection) + if localization is None: + return SkillResult.fail("OBJECT_NOT_DETECTED", f"Unknown selection: {selection}") try: - pointcloud = self._scene.get_object_pointcloud_by_object_id(object_id) - if pointcloud is None: + if localization.point_cloud is None: return SkillResult.fail( - "OBJECT_NOT_DETECTED", f"No pointcloud for object_id: {object_id}" + "OBJECT_NOT_DETECTED", f"No pointcloud for selection: {selection}" ) - candidates = self._grasp_generator.propose_grasps(pointcloud) + candidates = self._grasp_generator.propose_grasps(localization.point_cloud) except (RuntimeError, ValueError) as exc: return SkillResult.fail("GRASP_GENERATION_FAILED", str(exc)) self._grasp_candidates = candidates @@ -161,14 +168,14 @@ def pick_object( if failure := self._close_and_verify(group): return failure - self._selected_object_id = object_id + self._selected_object = selection self._selected_grasp = grasp self._holding_object = True if failure := self._move(pregrasp, group): return failure return SkillResult.ok( "Pick complete", - object_id=object_id, + selection=selection, rank=0, score=candidate.score, candidates=len(candidates.candidates), @@ -219,7 +226,7 @@ def place_at( def _clear_selection(self) -> None: self._grasp_candidates = GraspCandidateArray() - self._selected_object_id = None + self._selected_object = None self._selected_grasp = None def _resolve_group(self, planning_group: PlanningGroupID | None) -> PlanningGroupID | None: diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index b8f3700887..da11910bd1 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -25,7 +25,6 @@ from dimos.manipulation.planning import factory as planning_factory from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.monitor import world_monitor as world_monitor_module -from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( @@ -36,15 +35,11 @@ VisualizationStateFrame, ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec -from dimos.manipulation.planning.utils import mesh_utils from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.JointState import JointState -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.perception.experimental.object import Object from dimos.robot.assets.model import RobotModel @@ -737,34 +732,17 @@ def test_world_monitor_routes_obstacle_sources_and_empty_monitor_operations( ) -> None: monitor = world_monitor_module.WorldMonitor(world=FakeWorld()) # type: ignore[arg-type] detections = [object()] - objects = [object()] - assert monitor.refresh_obstacles() == [] - assert monitor.remove_object_obstacle("missing") is False - assert monitor.clear_perception_obstacles() == 0 monitor.on_collision_object(CollisionObjectMessage(id="id", operation="add")) monitor.on_detections(detections) # type: ignore[arg-type] - monitor.on_objects(objects) monitor.start_obstacle_monitor() obstacle_monitor = monitor.obstacle_monitor assert obstacle_monitor is not None - refresh = mocker.patch.object(obstacle_monitor, "refresh_obstacles", return_value=[{"id": "x"}]) - remove = mocker.patch.object(obstacle_monitor, "remove_object_obstacle", return_value=True) - clear = mocker.patch.object(obstacle_monitor, "clear_perception_obstacles", return_value=2) on_detections = mocker.patch.object(obstacle_monitor, "on_detections") - on_objects = mocker.patch.object(obstacle_monitor, "on_objects") - assert monitor.refresh_obstacles(0.5) == [{"id": "x"}] - assert monitor.remove_object_obstacle("object-id") is True - assert monitor.clear_perception_obstacles() == 2 monitor.on_detections(detections) # type: ignore[arg-type] - monitor.on_objects(objects) - refresh.assert_called_once_with(0.5) - remove.assert_called_once_with("object-id") - clear.assert_called_once_with() on_detections.assert_called_once_with(detections) - on_objects.assert_called_once_with(objects) def test_world_obstacle_monitor_rejects_invalid_add_and_handles_update_and_callbacks( @@ -831,87 +809,3 @@ def test_world_obstacle_monitor_detection_add_update_and_stale_cleanup( update.assert_called_once_with("first-id", mocker.ANY) remove.assert_called_once_with("first-id") assert obstacle_monitor.get_obstacle_count() == 0 - - -def _tilted_box_cloud() -> np.ndarray: - """Single-view cloud of a 20x8x8 cm box tilted 30 degrees off the world axes.""" - rng = np.random.default_rng(0) - length, width, height = 0.20, 0.08, 0.08 - u = rng.uniform(-0.5, 0.5, 4000) - v = rng.uniform(-0.5, 0.5, 4000) - # Two visible faces only, so the cloud mean sits off the box center. - front = np.column_stack([u * length, np.full(u.size, -width / 2), v * height]) - top = np.column_stack([u * length, v * width, np.full(u.size, height / 2)]) - angle = np.deg2rad(30.0) - rotation = np.array( - [ - [np.cos(angle), -np.sin(angle), 0.0], - [np.sin(angle), np.cos(angle), 0.0], - [0.0, 0.0, 1.0], - ] - ) - return np.vstack([front, top]) @ rotation.T + np.array([1.0, 0.5, 0.9]) - - -def test_mesh_obstacle_is_placed_at_the_hull_centroid_without_the_bbox_rotation( - tmp_path: Path, - monkeypatch: pytest.MonkeyPatch, - mocker: MockerFixture, -) -> None: - monkeypatch.setattr(mesh_utils, "_CACHE_DIR", tmp_path / "derived" / "drake_meshes") - parent = world_monitor_module.WorldMonitor(world=FakeWorld()) # type: ignore[arg-type] - add_obstacle = mocker.patch.object(parent, "add_obstacle", return_value="parent-id") - - points = _tilted_box_cloud() - cloud = PointCloud2.from_numpy(points, frame_id="world") - # Mirrors Object.from_detections with use_aabb=False: pose carries the - # oriented-box center and rotation. - obb = cloud.pointcloud.get_oriented_bounding_box() - obj = Object( - object_id="tilted-box", - name="box", - center=Vector3(obb.center), - size=Vector3(obb.extent), - pose=PoseStamped( - frame_id="world", - position=Vector3(obb.center), - orientation=Quaternion.from_rotation_matrix(np.asarray(obb.R)), - ), - pointcloud=cloud, - bbox=(0.0, 0.0, 1.0, 1.0), - track_id=0, - class_id=0, - confidence=1.0, - ts=0.0, - image=Image(), - ) - - monitor = WorldObstacleMonitor(parent=parent, use_mesh_obstacles=True) - monitor.start() - monitor.on_objects([obj]) - monitor.refresh_obstacles() - - obstacle = add_obstacle.call_args.args[0] - centroid = points.mean(axis=0) - assert obstacle.obstacle_type == ObstacleType.MESH - np.testing.assert_allclose( - [obstacle.pose.position.x, obstacle.pose.position.y, obstacle.pose.position.z], - centroid, - atol=1e-3, - ) - np.testing.assert_allclose( - [ - obstacle.pose.orientation.x, - obstacle.pose.orientation.y, - obstacle.pose.orientation.z, - obstacle.pose.orientation.w, - ], - [0.0, 0.0, 0.0, 1.0], - atol=1e-9, - ) - assert obstacle.pose.frame_id == "world" - - # Teeth: the pose the old code used is a genuinely different placement, so - # this cannot pass on the bug. - assert np.linalg.norm(np.asarray(obb.center) - centroid) > 3e-3 - assert abs(Quaternion.from_rotation_matrix(np.asarray(obb.R)).w) < 0.999 diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index e0a3cb4ab9..24e6ac80b8 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -52,7 +52,6 @@ PlanningGroupID, ) from dimos.msgs.vision_msgs.Detection3D import Detection3D - from dimos.perception.experimental.object import Object logger = setup_logger() @@ -251,53 +250,6 @@ def on_detections(self, detections: list[Detection3D]) -> None: if self._obstacle_monitor is not None: self._obstacle_monitor.on_detections(detections) - def on_objects(self, objects: object) -> None: - """Handle Object detections from ObjectDB (preserves object_id).""" - if self._obstacle_monitor is not None and isinstance(objects, list): - self._obstacle_monitor.on_objects(objects) - - def refresh_obstacles(self, min_duration: float = 0.0) -> list[dict[str, Any]]: - """Refresh perception obstacles from cache. Returns list of added obstacles.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.refresh_obstacles(min_duration) - return [] - - def remove_object_obstacle(self, object_id: str) -> bool: - """Remove a single object's obstacle from the planning world.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.remove_object_obstacle(object_id) - return False - - def clear_perception_obstacles(self) -> int: - """Remove all perception obstacles. Returns count removed.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.clear_perception_obstacles() - return 0 - - def get_perception_status(self) -> dict[str, int]: - """Get perception obstacle status.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.get_perception_status() - return {"cached": 0, "added": 0} - - def get_cached_objects(self) -> list[Object]: - """Get cached Object instances from perception.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.get_cached_objects() - return [] - - def list_cached_detections(self) -> list[dict[str, Any]]: - """List cached detections from perception.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.list_cached_detections() - return [] - - def list_added_obstacles(self) -> list[dict[str, Any]]: - """List perception obstacles currently in the planning world.""" - if self._obstacle_monitor is not None: - return self._obstacle_monitor.list_added_obstacles() - return [] - # State Access def get_current_joint_state(self) -> JointState | None: diff --git a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py index 41ff2b2fbc..732081548b 100644 --- a/dimos/manipulation/planning/monitor/world_obstacle_monitor.py +++ b/dimos/manipulation/planning/monitor/world_obstacle_monitor.py @@ -28,7 +28,7 @@ from dataclasses import replace import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( @@ -37,8 +37,6 @@ Obstacle, ) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: @@ -46,7 +44,6 @@ from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.msgs.vision_msgs.Detection3D import Detection3D - from dimos.perception.experimental.object import Object logger = setup_logger() @@ -73,31 +70,22 @@ def __init__( self, parent: WorldMonitor, detection_timeout: float = 2.0, - use_mesh_obstacles: bool = False, ) -> None: """Create a world obstacle monitor. Args: parent: Owning WorldMonitor instance detection_timeout: Time before removing stale detections (seconds) - use_mesh_obstacles: Use convex hull meshes from pointclouds instead of bounding boxes """ self._parent = parent self._lock = parent._lock self._detection_timeout = detection_timeout - self._use_mesh_obstacles = use_mesh_obstacles # Track obstacles from different sources self._collision_objects: dict[str, str] = {} # msg_id -> obstacle_id self._perception_objects: dict[str, str] = {} # detection_id -> obstacle_id self._perception_timestamps: dict[str, float] = {} # detection_id -> timestamp - # Object-based cache (from ObjectDB, keyed by object_id) - # object_id -> (Object, first_seen, last_seen) - self._object_cache: dict[str, tuple[Object, float, float]] = {} - # object_id -> obstacle_id (objects currently added to Drake world) - self._object_obstacles: dict[str, str] = {} - # Running state self._running = False @@ -119,12 +107,11 @@ def is_running(self) -> bool: return self._running def clear_tracking(self) -> None: - """Forget IDs for cleared obstacles while retaining the object cache.""" + """Forget IDs for cleared obstacles.""" with self._lock: self._collision_objects.clear() self._perception_objects.clear() self._perception_timestamps.clear() - self._object_obstacles.clear() def on_collision_object(self, msg: CollisionObjectMessage) -> None: """Handle explicit collision object message. @@ -445,235 +432,3 @@ def remove_obstacle_callback( """Remove an obstacle callback.""" if callback in self._obstacle_callbacks: self._obstacle_callbacks.remove(callback) - - # Object-Based Perception (from ObjectDB) - - def on_objects(self, objects: list[object]) -> None: - """Cache objects from ObjectDB (preserves stable object_id). - - Unlike on_detections(), this receives Object instances with stable IDs - from ObjectDB deduplication, making the cache trivially keyed by object_id. - - Args: - objects: List of Object instances from ObjectDB - """ - if not self._running: - return - - from dimos.perception.experimental.object import Object - - now = time.time() - seen: set[str] = set() - - with self._lock: - for obj in objects: - if not isinstance(obj, Object): - continue - oid = obj.object_id - seen.add(oid) - if oid in self._object_cache: - _, first, _ = self._object_cache[oid] - self._object_cache[oid] = (obj, first, now) - else: - self._object_cache[oid] = (obj, now, now) - - # Remove objects no longer reported by ObjectDB - stale = [oid for oid in self._object_cache if oid not in seen] - for oid in stale: - del self._object_cache[oid] - - def refresh_obstacles(self, min_duration: float = 0.0) -> list[dict[str, Any]]: - """Full sync: remove all object obstacles, re-add from cache. - - Args: - min_duration: Minimum seconds an object must have been seen to be included - - Returns: - List of added obstacles with object_id, obstacle_id, name, center, size - """ - from dimos.perception.experimental.object import Object - - # Step 1: snapshot eligible objects under lock (fast) - eligible: list[tuple[str, Object]] = [] - with self._lock: - for oid, (obj, first_seen, last_seen) in self._object_cache.items(): - if not isinstance(obj, Object): - continue - if last_seen - first_seen < min_duration: - continue - eligible.append((oid, obj)) - - # Step 2: compute obstacles OUTSIDE lock (convex hull can be slow) - prepared: list[tuple[str, Object, Obstacle]] = [] - for oid, obj in eligible: - obstacle = self._object_to_obstacle(obj) - prepared.append((oid, obj, obstacle)) - - # Step 3: apply to Drake world under lock (fast) - with self._lock: - # TODO: Diff stable ObjectDB IDs and update existing obstacles - # instead of removing and re-adding the complete set. - for obs_id in self._object_obstacles.values(): - self._parent.remove_obstacle(obs_id) - self._object_obstacles.clear() - - result: list[dict[str, Any]] = [] - for oid, obj, obstacle in prepared: - assert isinstance(obj, Object) - obs_id = self._parent.add_obstacle(obstacle) - if not obs_id: - continue - self._object_obstacles[oid] = obs_id - result.append( - { - "object_id": oid, - "obstacle_id": obs_id, - "name": obj.name, - "center": [float(obj.center.x), float(obj.center.y), float(obj.center.z)], - "size": [float(obj.size.x), float(obj.size.y), float(obj.size.z)], - } - ) - logger.debug(f"Added object obstacle '{oid}' ({obj.name}) as '{obs_id}'") - - return result - - def remove_object_obstacle(self, object_id: str) -> bool: - """Remove a single object's obstacle from the planning world. - - Args: - object_id: The exact object_id to remove. - - Returns: - True if found and removed, False otherwise. - """ - with self._lock: - obs_id = self._object_obstacles.pop(object_id, None) - if obs_id is None: - return False - self._parent.remove_obstacle(obs_id) - logger.info(f"Removed obstacle for object '{object_id}'") - return True - - def clear_perception_obstacles(self) -> int: - """Remove all object obstacles from the planning world. - - Returns: - Number of obstacles removed - """ - with self._lock: - count = len(self._object_obstacles) - for obs_id in self._object_obstacles.values(): - self._parent.remove_obstacle(obs_id) - self._object_obstacles.clear() - return count - - def get_perception_status(self) -> dict[str, int]: - """Get perception obstacle status.""" - with self._lock: - return { - "cached": len(self._object_cache), - "added": len(self._object_obstacles), - } - - def get_cached_objects(self) -> list[Object]: - """Get cached Object instances from perception. - - Returns raw Object instances for typed access to .name, .center, .size etc. - """ - from dimos.perception.experimental.object import Object as _Object - - with self._lock: - return [obj for obj, _, _ in self._object_cache.values() if isinstance(obj, _Object)] - - def list_cached_detections(self) -> list[dict[str, Any]]: - """List cached detections from perception.""" - from dimos.perception.experimental.object import Object - - with self._lock: - result: list[dict[str, Any]] = [] - for oid, (obj, first_seen, last_seen) in self._object_cache.items(): - if not isinstance(obj, Object): - continue - result.append( - { - "object_id": oid, - "name": obj.name, - "center": [float(obj.center.x), float(obj.center.y), float(obj.center.z)], - "size": [float(obj.size.x), float(obj.size.y), float(obj.size.z)], - "duration": round(last_seen - first_seen, 1), - "in_world": oid in self._object_obstacles, - } - ) - return result - - def list_added_obstacles(self) -> list[dict[str, Any]]: - """List perception obstacles currently in the planning world.""" - from dimos.perception.experimental.object import Object - - with self._lock: - result: list[dict[str, Any]] = [] - for oid, obs_id in self._object_obstacles.items(): - entry = self._object_cache.get(oid) - if entry is None: - continue - obj, _first_seen, _last_seen = entry - if not isinstance(obj, Object): - continue - result.append( - { - "object_id": oid, - "obstacle_id": obs_id, - "name": obj.name, - "center": [float(obj.center.x), float(obj.center.y), float(obj.center.z)], - "size": [float(obj.size.x), float(obj.size.y), float(obj.size.z)], - } - ) - return result - - def _object_to_obstacle(self, obj: object) -> Obstacle: - """Convert Object to obstacle. Uses bounding box by default, convex hull if use_mesh_obstacles=True.""" - from dimos.perception.experimental.object import Object - - assert isinstance(obj, Object) - name = f"object_{obj.object_id}" - - # Try convex hull from pointcloud (opt-in) - if self._use_mesh_obstacles and obj.pointcloud is not None: - try: - from dimos.manipulation.planning.utils.mesh_utils import ( - pointcloud_to_convex_hull_obj, - ) - - points, _ = obj.pointcloud.as_numpy() - if points is not None and points.shape[0] >= 4: - # Keyed on the object's stable unique id: rescans overwrite - # in place, and no two objects share a file. - hull = pointcloud_to_convex_hull_obj(points, cache_key=name) - if hull is not None: - # The hull is world-axis-aligned about its own centroid, - # so that is the only pose that leaves it on its points. - # obj.pose carries the bbox center and the oriented-box - # rotation, neither of which the vertices were built from. - return Obstacle( - name=name, - obstacle_type=ObstacleType.MESH, - pose=PoseStamped( - ts=obj.pose.ts, - frame_id=obj.pose.frame_id, - position=Vector3(hull.centroid), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - color=(0.2, 0.8, 0.2, 0.6), - mesh_path=hull.path, - ) - except Exception as e: - logger.debug(f"Convex hull failed for {name}, falling back to box: {e}") - - # Default: bounding box - return Obstacle( - name=name, - obstacle_type=ObstacleType.BOX, - pose=obj.pose or PoseStamped(position=obj.center), - dimensions=(float(obj.size.x), float(obj.size.y), float(obj.size.z)), - color=(0.2, 0.8, 0.2, 0.6), - ) diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index 2009dfb6a6..5799b7b414 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -132,7 +132,6 @@ def module(xarm7_config): mod._control_coordinator = coordinator mod.coordinator_joint_state = None mod.voxel_map = None - mod.objects = None mod.start() yield mod mod.stop() diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 6638696206..359d17a9cc 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -281,21 +281,15 @@ def test_a_rejected_map_leaves_the_previous_one_standing(self, module_factory) - class TestObstacleUpdates: - async def test_perception_objects_are_refreshable_and_queryable(self, module_factory) -> None: + def test_obstacles_are_queryable(self, module_factory) -> None: module = module_factory() module._world_monitor = MagicMock(spec=WorldMonitor) - detected = MagicMock() pose = PoseStamped(position=Vector3(0.4, 0.1, 0.2)) obstacle = Obstacle(name="object-1", pose=pose, obstacle_type=ObstacleType.BOX) - module._world_monitor.refresh_obstacles.return_value = [{"object_id": "object-1"}] module._world_monitor.world.get_obstacles.return_value = [obstacle] - await module.handle_objects([detected]) - count = module.refresh_obstacles() obstacles = module.get_obstacles() - module._world_monitor.on_objects.assert_called_once_with([detected]) - assert count == 1 assert obstacles == {"object-1": pose} def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> None: @@ -482,7 +476,6 @@ def test_start_eagerly_initializes_planning_and_execution( module = ManipulationModule(model=robot_config) module.coordinator_joint_state = None module.voxel_map = None - module.objects = None initialize_planning = mocker.patch.object(module, "_initialize_planning") initialize_execution = mocker.patch.object(module, "_initialize_execution") @@ -494,7 +487,6 @@ def test_start_is_idempotent(self, mocker: MockerFixture, robot_config) -> None: module = ManipulationModule(model=robot_config) module.coordinator_joint_state = None module.voxel_map = None - module.objects = None initialize_planning = mocker.patch.object(module, "_initialize_planning") initialize_execution = mocker.patch.object(module, "_initialize_execution") @@ -516,7 +508,6 @@ def test_state_is_readable_during_planning_initialization( module._control_coordinator = _control_coordinator() module.coordinator_joint_state = None module.voxel_map = None - module.objects = None observed_status: list[ExecutionStatus] = [] def observe_state() -> None: diff --git a/dimos/manipulation/test_pick_and_place_unit.py b/dimos/manipulation/test_pick_and_place_unit.py index 2a28083cc5..d01fc392cf 100644 --- a/dimos/manipulation/test_pick_and_place_unit.py +++ b/dimos/manipulation/test_pick_and_place_unit.py @@ -29,6 +29,7 @@ from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate from dimos.msgs.manipulation_msgs.GraspCandidateArray import GraspCandidateArray from dimos.msgs.std_msgs.Header import Header +from dimos.perception.memory.types import Localization @pytest.fixture @@ -55,8 +56,9 @@ def module() -> Iterator[PickAndPlaceModule]: instance._manipulation.set_gripper_position.return_value = SimpleNamespace( succeeded=True, message="" ) - instance._objects = {"cup-1": {"object_id": "cup-1", "name": "cup"}} - instance._scene.get_object_pointcloud_by_object_id.return_value = MagicMock() + localization = _localization() + instance._objects = {0: {"selection": 0, "name": "cup", "score": 0.9}} + instance._localizations = {0: localization} instance._grasp_generator.propose_grasps.return_value = GraspCandidateArray( Header(1.0, "world"), [_candidate(0.1)] ) @@ -80,22 +82,59 @@ def _candidate(x: float, score: float = 1.0) -> GraspCandidate: ) -def test_scan_objects_uses_latest_scan_ids(module: PickAndPlaceModule) -> None: - scene: Any = module._scene - scene.scan_scene.return_value = SimpleNamespace( - detections_length=1, - detections=[ - SimpleNamespace( - id="cup-1", results=[SimpleNamespace(hypothesis=SimpleNamespace(class_id="cup"))] - ) - ], +def _localization() -> Localization: + return Localization( + instance_id="query-0", + semantic_score=0.9, + identity_score=0.5, + ambiguity_margin=1.0, + position_world_xyz=(0.1, 0.0, 0.2), + orientation_world_xyzw=(0.0, 0.0, 0.0, 1.0), + frame_id="world", + support=None, + pose_timestamp=1.0, + geometry_timestamp=1.0, + last_seen_timestamp=1.0, + point_cloud=MagicMock(), + cloud_mode="latest_visible", + coverage=0.5, + n_views=2, ) + +def test_scan_objects_caches_latest_localizations_by_selection( + module: PickAndPlaceModule, +) -> None: + scene: Any = module._scene + localization = _localization() + scene.localize_objects.return_value = [localization] + result = module.scan_objects([" cup "]) assert result.is_success() - assert module.get_object("cup-1") == {"object_id": "cup-1", "name": "cup"} - scene.scan_scene.assert_called_once_with(text=["cup"]) + assert module.get_object(0) == {"selection": 0, "name": "cup", "score": 0.9} + assert module._localizations == {0: localization} + scene.localize_objects.assert_called_once_with(["cup"]) + + +def test_scan_objects_omits_misses_and_assigns_dense_selections( + module: PickAndPlaceModule, +) -> None: + localization = _localization() + module._scene.localize_objects.return_value = [None, localization] + + result = module.scan_objects(["cup", "bowl"]) + + assert result.is_success() + assert module.get_object(0) == {"selection": 0, "name": "bowl", "score": 0.9} + assert module.get_object(1) is None + + +def test_scan_objects_rejects_duplicate_prompts(module: PickAndPlaceModule) -> None: + result = module.scan_objects(["cup", " cup "]) + + assert result.error_code == "INVALID_INPUT" + module._scene.localize_objects.assert_not_called() def test_pick_object_uses_first_provider_candidate( @@ -107,7 +146,7 @@ def test_pick_object_uses_first_provider_candidate( Header(1.0, "world"), [first, second] ) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.is_success() assert module.get_grasp_candidates().candidates == [first, second] @@ -116,13 +155,25 @@ def test_pick_object_uses_first_provider_candidate( assert result.metadata["rank"] == 0 +def test_pick_object_uses_cached_scan_cloud_without_perception_lookup( + module: PickAndPlaceModule, +) -> None: + localization = module._localizations[0] + + result = module.pick_object(0) + + assert result.is_success() + module._grasp_generator.propose_grasps.assert_called_once_with(localization.point_cloud) + module._scene.assert_not_called() + + def test_pick_object_rejects_non_planning_frame(module: PickAndPlaceModule) -> None: grasp_generator: Any = module._grasp_generator grasp_generator.propose_grasps.return_value = GraspCandidateArray( Header(1.0, "camera"), [_candidate(0.1)] ) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert not result.is_success() assert result.error_code == "GRASP_FRAME_MISMATCH" @@ -133,7 +184,7 @@ def test_pick_object_rejects_empty_candidates(module: PickAndPlaceModule) -> Non Header(1.0, "world"), [] ) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRASP_GENERATION_FAILED" module._manipulation.set_gripper_position.assert_not_called() @@ -154,7 +205,7 @@ def test_pick_preserves_current_yaw_when_configured(module: PickAndPlaceModule) ], ) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.is_success() assert module._selected_grasp is not None @@ -182,14 +233,14 @@ def test_place_uses_local_axis_and_clears_held_state(module: PickAndPlaceModule) def test_scan_failure_clears_stale_selection(module: PickAndPlaceModule) -> None: scene: Any = module._scene module._selected_grasp = PoseStamped(frame_id="world") - scene.scan_scene.side_effect = RuntimeError("No aligned RGB-D frame") + scene.localize_objects.side_effect = RuntimeError("No embedded RGB-D frame") result = module.scan_objects(["cup"]) assert not result.is_success() assert result.error_code == "PERCEPTION_FAILED" assert module._selected_grasp is None - assert module.get_object("cup-1") is None + assert module.get_object(0) is None def test_pick_rejects_when_already_holding(module: PickAndPlaceModule) -> None: @@ -197,7 +248,7 @@ def test_pick_rejects_when_already_holding(module: PickAndPlaceModule) -> None: module._holding_object = True module._selected_grasp = PoseStamped(frame_id="world") - pick = module.pick_object("cup-1") + pick = module.pick_object(0) assert pick.error_code == "INVALID_STATE" manipulation.set_gripper_position.assert_not_called() @@ -206,7 +257,7 @@ def test_pick_rejects_when_already_holding(module: PickAndPlaceModule) -> None: def test_failed_pick_clears_previous_selection(module: PickAndPlaceModule) -> None: module._selected_grasp = PoseStamped(frame_id="world") - result = module.pick_object("missing") + result = module.pick_object(9) assert result.error_code == "OBJECT_NOT_DETECTED" assert module._selected_grasp is None @@ -220,7 +271,7 @@ def test_pick_retains_held_state_when_retract_fails(module: PickAndPlaceModule) SimpleNamespace(succeeded=False, message="retract failed"), ] - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "EXECUTION_FAILED" assert module._holding_object @@ -237,7 +288,7 @@ def settle(read: Any, target: float, config: Any) -> GripperSettle: monkeypatch.setattr("dimos.manipulation.pick_and_place_module.await_gripper_settle", settle) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRASP_VERIFICATION_FAILED" assert not module._holding_object @@ -253,7 +304,7 @@ def settle(read: Any, target: float, config: Any) -> GripperSettle: monkeypatch.setattr("dimos.manipulation.pick_and_place_module.await_gripper_settle", settle) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRASP_VERIFICATION_FAILED" assert not module._holding_object @@ -275,7 +326,7 @@ def settle(read: Any, target: float, config: Any) -> GripperSettle: monkeypatch.setattr("dimos.manipulation.pick_and_place_module.await_gripper_settle", settle) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRIPPER_FAILED" assert "recovery open failed" in result.message @@ -287,7 +338,7 @@ def test_pick_fails_when_gripper_command_is_rejected(module: PickAndPlaceModule) succeeded=False, message="controller unavailable" ) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRIPPER_FAILED" @@ -302,7 +353,7 @@ def settle(read: Any, target: float, config: Any) -> GripperSettle: monkeypatch.setattr("dimos.manipulation.pick_and_place_module.await_gripper_settle", settle) - result = module.pick_object("cup-1") + result = module.pick_object(0) assert result.error_code == "GRASP_VERIFICATION_FAILED" assert not module._holding_object diff --git a/dimos/perception/experimental/demo_object_scene_registration.py b/dimos/perception/experimental/demo_object_scene_registration.py deleted file mode 100644 index dd85ab1d03..0000000000 --- a/dimos/perception/experimental/demo_object_scene_registration.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from dimos.agents.mcp.mcp_client import McpClient -from dimos.agents.mcp.mcp_server import McpServer -from dimos.core.coordination.blueprints import autoconnect -from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera -from dimos.hardware.sensors.camera.zed.compat import ZEDCamera -from dimos.perception.detection.detectors.yoloe import YoloePromptMode -from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule - -camera_choice = "zed" - -if camera_choice == "realsense": - camera_module = RealSenseCamera.blueprint(enable_pointcloud=False) -elif camera_choice == "zed": - camera_module = ZEDCamera.blueprint(enable_pointcloud=False) -else: - raise ValueError(f"Invalid camera choice: {camera_choice}") - -demo_object_scene_registration = autoconnect( - camera_module, - ObjectSceneRegistrationModule.blueprint(target_frame="world", prompt_mode=YoloePromptMode.LRPC), - McpServer.blueprint(), - McpClient.blueprint(), -) diff --git a/dimos/perception/experimental/objectDB.py b/dimos/perception/experimental/objectDB.py deleted file mode 100644 index d2d8e7e1be..0000000000 --- a/dimos/perception/experimental/objectDB.py +++ /dev/null @@ -1,340 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import threading -import time -from typing import TYPE_CHECKING, Any - -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.utils.logging_config import setup_logger - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - from dimos.perception.experimental.object import Object - -logger = setup_logger() - - -class ObjectDB: - """Spatial memory database for 3D object detections. - - Maintains two tiers of objects internally: - - _pending_objects: Recently detected objects (detection_count < threshold) - - _objects: Confirmed permanent objects (detection_count >= threshold) - - Deduplication uses two heuristics: - 1. track_id match from YOLOE tracker (recent match) - 2. Center distance within threshold (spatial proximity match) - """ - - def __init__( - self, - distance_threshold: float = 0.2, - min_detections_for_permanent: int = 6, - pending_ttl_s: float = 5.0, - track_id_ttl_s: float = 5.0, - ) -> None: - self._distance_threshold = distance_threshold - self._min_detections = min_detections_for_permanent - self._pending_ttl_s = pending_ttl_s - self._track_id_ttl_s = track_id_ttl_s - - # Internal storage - keyed by object_id - self._pending_objects: dict[str, Object] = {} - self._objects: dict[str, Object] = {} # Permanent objects - - # track_id -> object_id mapping for fast lookup - self._track_id_map: dict[int, str] = {} - self._last_add_stats: dict[str, int] = {} - - self._lock = threading.RLock() - - # ───────────────────────────────────────────────────────────────── - # Public Methods - # ───────────────────────────────────────────────────────────────── - - def add_objects(self, objects: list[Object]) -> list[Object]: - """Add multiple objects to the database with deduplication. - - Args: - objects: List of Object instances from object_scene_registration - - Returns: - List of updated/created Object instances - """ - stats = { - "input": len(objects), - "created": 0, - "updated": 0, - "promoted": 0, - "matched_track": 0, - "matched_distance": 0, - } - - results: dict[str, Object] = {} - now = time.time() - with self._lock: - self._prune_stale_pending(now) - for obj in objects: - matched, reason = self._match(obj, now) - if matched is None: - inserted = self._insert_pending(obj, now) - results[inserted.object_id] = inserted - stats["created"] += 1 - if self._check_promotion(inserted): - stats["promoted"] += 1 - continue - - updated = self._update_existing(matched, obj, now) - results[matched.object_id] = matched - if updated: - stats["updated"] += 1 - if reason == "track": - stats["matched_track"] += 1 - elif reason == "distance": - stats["matched_distance"] += 1 - if updated and self._check_promotion(matched): - stats["promoted"] += 1 - - stats["pending"] = len(self._pending_objects) - stats["permanent"] = len(self._objects) - self._last_add_stats = stats - if stats["created"] > 0 or stats["promoted"] > 0: - logger.info(f"ObjectDB: {stats}") - return list(results.values()) - - def get_last_add_stats(self) -> dict[str, int]: - with self._lock: - return dict(self._last_add_stats) - - def get_objects(self) -> list[Object]: - """Get all permanent objects (detection_count >= threshold).""" - with self._lock: - return list(self._objects.values()) - - def get_all_objects(self) -> list[Object]: - """Get all objects (both pending and permanent).""" - with self._lock: - return list(self._pending_objects.values()) + list(self._objects.values()) - - def promote(self, object_id: str) -> bool: - """Promote an object from pending to permanent.""" - with self._lock: - if object_id in self._pending_objects: - self._objects[object_id] = self._pending_objects.pop(object_id) - return True - return object_id in self._objects - - def find_by_name(self, name: str) -> list[Object]: - """Find all permanent objects with matching name.""" - with self._lock: - return [obj for obj in self._objects.values() if obj.name == name] - - def find_by_object_id(self, object_id: str) -> Object | None: - """Find an object by its object_id (searches pending and permanent).""" - with self._lock: - if object_id in self._objects: - return self._objects[object_id] - if object_id in self._pending_objects: - return self._pending_objects[object_id] - return None - - def find_nearest( - self, - position: Vector3, - name: str | None = None, - ) -> Object | None: - """Find nearest permanent object to a position, optionally filtered by name. - - Args: - position: Position to search from - name: Optional name filter - - Returns: - Nearest Object or None if no objects found - """ - with self._lock: - candidates = [ - obj - for obj in self._objects.values() - if obj.center is not None and (name is None or obj.name == name) - ] - - if not candidates: - return None - - return min(candidates, key=lambda obj: position.distance(obj.center)) - - def clear(self) -> None: - """Clear all objects from the database.""" - import open3d as o3d # type: ignore[import-untyped] - - with self._lock: - # Drop Open3D pointcloud references before clearing to reduce shutdown warnings. - for obj in list(self._pending_objects.values()) + list(self._objects.values()): - obj.pointcloud = PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=obj.pointcloud.frame_id, - ts=obj.pointcloud.ts, - ) - self._pending_objects.clear() - self._objects.clear() - self._track_id_map.clear() - logger.info("ObjectDB cleared") - - def get_stats(self) -> dict[str, int]: - """Get statistics about the database.""" - with self._lock: - return { - "pending_count": len(self._pending_objects), - "permanent_count": len(self._objects), - "total_count": len(self._pending_objects) + len(self._objects), - } - - # ───────────────────────────────────────────────────────────────── - # Internal Methods - # ───────────────────────────────────────────────────────────────── - - def _match(self, obj: Object, now: float) -> tuple[Object | None, str | None]: - if obj.track_id >= 0: - matched = self._match_by_track_id(obj.track_id, now) - if matched is not None: - return matched, "track" - - matched = self._match_by_distance(obj) - if matched is not None: - return matched, "distance" - return None, None - - def _insert_pending(self, obj: Object, now: float) -> Object: - if not obj.ts: - obj.ts = now - obj.last_seen_ts = now - self._pending_objects[obj.object_id] = obj - if obj.track_id >= 0: - self._track_id_map[obj.track_id] = obj.object_id - logger.info(f"Created new pending object {obj.object_id} ({obj.name})") - return obj - - def _update_existing(self, existing: Object, obj: Object, now: float) -> bool: - if obj.track_id >= 0: - self._track_id_map[obj.track_id] = existing.object_id - # Multiple prompts or repeated scans may produce the same object from - # one camera frame. Only distinct source observations advance memory. - if existing.ts == obj.ts: - return False - - existing.update_object(obj) - existing.ts = obj.ts or now - existing.last_seen_ts = now - return True - - def _match_by_track_id(self, track_id: int, now: float) -> Object | None: - """Find object with matching track_id from YOLOE.""" - if track_id < 0: - return None - - object_id = self._track_id_map.get(track_id) - if object_id is None: - return None - - # Check in permanent objects first - if object_id in self._objects: - obj = self._objects[object_id] - elif object_id in self._pending_objects: - obj = self._pending_objects[object_id] - else: - del self._track_id_map[track_id] - return None - - last_seen = obj.last_seen_ts if obj.last_seen_ts is not None else now - if now - last_seen > self._track_id_ttl_s: - del self._track_id_map[track_id] - return None - - return obj - - def _match_by_distance(self, obj: Object) -> Object | None: - """Find object within distance threshold (name-agnostic). - - Name matching is intentionally excluded because YOLO labels are - unstable across frames — the same physical object may be called - "sharpener" one frame and "spray can" the next. With a tight - distance threshold (5cm), two distinct objects at the same spot - is effectively impossible. - """ - if obj.center is None: - return None - - all_objects = list(self._objects.values()) + list(self._pending_objects.values()) - candidates = [ - o - for o in all_objects - if o.center is not None and obj.center.distance(o.center) < self._distance_threshold - ] - - if not candidates: - return None - - return min(candidates, key=lambda o: obj.center.distance(o.center)) - - def _prune_stale_pending(self, now: float) -> None: - if self._pending_ttl_s <= 0: - return - cutoff = now - self._pending_ttl_s - stale_ids = [ - obj_id - for obj_id, obj in self._pending_objects.items() - if (obj.last_seen_ts if obj.last_seen_ts is not None else now) < cutoff - ] - for obj_id in stale_ids: - del self._pending_objects[obj_id] - for track_id, mapped_id in list(self._track_id_map.items()): - if mapped_id == obj_id: - del self._track_id_map[track_id] - - def _check_promotion(self, obj: Object) -> bool: - """Move object from pending to permanent if threshold met.""" - if obj.detections_count >= self._min_detections: - # Check if it's in pending - if obj.object_id in self._pending_objects: - # Promote to permanent - del self._pending_objects[obj.object_id] - self._objects[obj.object_id] = obj - logger.info( - f"Promoted object {obj.object_id} ({obj.name}) to permanent " - f"with {obj.detections_count} detections" - ) - return True - return False - - # ───────────────────────────────────────────────────────────────── - # Agent encoding - # ───────────────────────────────────────────────────────────────── - - def agent_encode(self) -> list[dict[str, Any]]: - """Encode permanent objects for agent consumption.""" - with self._lock: - return [obj.agent_encode() for obj in self._objects.values()] - - def __len__(self) -> int: - """Return number of permanent objects.""" - with self._lock: - return len(self._objects) - - def __repr__(self) -> str: - with self._lock: - return f"ObjectDB(permanent={len(self._objects)}, pending={len(self._pending_objects)})" diff --git a/dimos/perception/experimental/object_scene_registration.py b/dimos/perception/experimental/object_scene_registration.py index 3eb7e86405..da7cc2c18b 100644 --- a/dimos/perception/experimental/object_scene_registration.py +++ b/dimos/perception/experimental/object_scene_registration.py @@ -15,488 +15,217 @@ from __future__ import annotations import threading -import time -from typing import Any, Literal +from typing import Any -import numpy as np -from numpy.typing import NDArray +from pydantic import Field -from dimos.agents.annotation import skill from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import In, Out -from dimos.models.segmentation.edge_tam import BoxPromptImageSegmenter -from dimos.models.segmentation.yoloe import YoloeBoxSegmenter -from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.core.stream import In +from dimos.memory.observationstore.memory import ListObservationStore +from dimos.memory.store.memory import MemoryStore +from dimos.memory.stream import Stream from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Header import Header +from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.vision_msgs.Detection2DArray import Detection2DArray -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray -from dimos.perception.detection.detectors.yoloe import Yoloe2DDetector, YoloePromptMode -from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.perception.experimental.object import ( - Object, - Object as DetObject, - aggregate_pointclouds, - to_detection3d_array, +from dimos.perception.experimental.object_scene_registration_spec import ( + ObjectSceneRegistrationSpec, ) -from dimos.perception.experimental.objectDB import ObjectDB -from dimos.types.timestamped import align_timestamped -from dimos.utils.logging_config import setup_logger -from dimos.utils.reactive import backpressure - -logger = setup_logger() +from dimos.perception.memory.dandetect import DanDetector +from dimos.perception.memory.types import Localization, LocalizePolicy class ObjectSceneRegistrationConfig(ModuleConfig): - target_frame: str = "base_link" - prompt_mode: YoloePromptMode = YoloePromptMode.LRPC - detector_backend: Literal["yoloe", "owlv2", "moondream"] = "yoloe" - segmentation_backend: Literal["yolo", "edgetam"] = "yolo" - detector_confidence: float = 0.6 - segmentation_confidence: float = 0.05 - detect_on_request: bool = False - distance_threshold: float = 0.2 - min_detections_for_permanent: int = 6 - max_distance: float = 0.0 - use_aabb: bool = False - max_obstacle_width: float = 0.0 - - -class ObjectSceneRegistrationModule(Module): - """Register prompted camera detections as stable 3D scene objects.""" + target_frame: str = "world" + optical_frame: str = "camera_color_optical_frame" + memory_window_seconds: float = Field(default=15.0, gt=0.0) + max_image_observations: int = Field(default=90, ge=1) + max_camera_info_observations: int = Field(default=10, ge=1) + max_tf_observations: int = Field(default=5000, ge=1) + candidate_floor: float = Field(default=0.25, ge=0.0, le=1.0) + accept_score: float = Field(default=0.4, ge=0.0, le=1.0) + refusal_margin: float = Field(default=0.15, ge=0.0, le=1.0) + min_views: int = Field(default=2, ge=1) + max_prompts_per_request: int = Field(default=12, ge=1) + max_result_age_seconds: float = Field(default=5.0, gt=0.0) + tf_tolerance_seconds: float = Field(default=0.12, ge=0.0) + + +class ObjectSceneRegistrationModule(Module, ObjectSceneRegistrationSpec): + """Localize prompted objects from a bounded, request-time scene history.""" color_image: In[Image] depth_image: In[Image] camera_info: In[CameraInfo] tf: In[TFMessage] - detections_2d: Out[Detection2DArray] - detections_3d: Out[Detection3DArray] - objects: Out[list[DetObject]] - pointcloud: Out[PointCloud2] - - _detector: Any | None = None - _segmenter: BoxPromptImageSegmenter | None = None - _camera_info: CameraInfo | None = None - _object_db: ObjectDB - _text_prompts: list[str] - _latest_aligned_frames: tuple[Image, Image] | None = None - _processing_lock: threading.RLock - # A tuple assignment/read is atomic, so depth and its transform cannot be - # observed from different frames by get_full_scene_pointcloud(). - _latest_scene_snapshot: tuple[Image, Transform | None] | None = None - config: ObjectSceneRegistrationConfig def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) - self._target_frame = self.config.target_frame - self._prompt_mode = self.config.prompt_mode - self._detector_backend = self.config.detector_backend - self._segmentation_backend = self.config.segmentation_backend - self._detector_confidence = self.config.detector_confidence - self._segmentation_confidence = self.config.segmentation_confidence - self._detect_on_request = self.config.detect_on_request - self._object_db = ObjectDB( - distance_threshold=self.config.distance_threshold, - min_detections_for_permanent=self.config.min_detections_for_permanent, + self._store: MemoryStore | None = None + self._detector: DanDetector | None = None + self._index: Stream[Any, Any] | None = None + self._state_lock = threading.RLock() + self._inference_lock = threading.Lock() + + @staticmethod + def _bounded_store(name: str, max_size: int) -> ListObservationStore[Any]: + return ListObservationStore(name=name, max_size=max_size) + + def _create_streams(self, store: MemoryStore) -> None: + store.stream("color_image", Image) + store.stream("depth_image", Image) + store.stream( + "camera_info", + CameraInfo, + observation_store=self._bounded_store( + "camera_info", self.config.max_camera_info_observations + ), ) - self._processing_lock = threading.RLock() - self._text_prompts = [] - self._max_distance = self.config.max_distance - self._use_aabb = self.config.use_aabb - self._max_obstacle_width = self.config.max_obstacle_width - - @rpc - def start(self) -> None: - super().start() - - if self._detector_backend == "owlv2": - from dimos.perception.detection.detectors.owlv2 import Owlv2Detector - - self._detector = Owlv2Detector() - self._detector.start() - elif self._detector_backend == "moondream": - from dimos.models.vl.moondream import MoondreamVlModel - - self._detector = MoondreamVlModel() - self._detector.start() - else: - if self._prompt_mode == YoloePromptMode.LRPC: - model_name = "yoloe-11l-seg-pf.pt" - else: - model_name = "yoloe-11l-seg.pt" - self._detector = Yoloe2DDetector( - model_name=model_name, - prompt_mode=self._prompt_mode, - conf=self._detector_confidence, - ) - - self._segmenter = self._create_segmenter() - - self.camera_info.subscribe(lambda msg: setattr(self, "_camera_info", msg)) - - aligned_frames = align_timestamped( - self.color_image.observable(), - self.depth_image.observable(), - buffer_size=2.0, - match_tolerance=0.1, + store.stream( + "tf", + TFMessage, + observation_store=self._bounded_store("tf", self.config.max_tf_observations), ) - backpressure(aligned_frames).subscribe(self._on_aligned_frames) - def _create_segmenter(self) -> BoxPromptImageSegmenter | None: - if self._segmentation_backend == "yolo": - if self._detector_backend == "yoloe": - return None - return YoloeBoxSegmenter(confidence=self._segmentation_confidence) + def _record_color_image(self, message: Image) -> None: + with self._state_lock: + store = self._store + if store is not None: + store.streams.color_image.append(message, ts=message.ts) + + def _record_depth_image(self, message: Image) -> None: + with self._state_lock: + store = self._store + if store is not None: + store.streams.depth_image.append(message, ts=message.ts) + + def _record_camera_info(self, message: CameraInfo) -> None: + with self._state_lock: + store = self._store + if store is not None: + store.streams.camera_info.append(message, ts=message.ts) + + def _record_tf(self, message: TFMessage) -> None: + with self._state_lock: + store = self._store + if store is None: + return - if self._segmentation_backend == "edgetam": - try: - from dimos.models.segmentation.edge_tam import EdgeTAMImageSegmenter + # Keep each edge at its own timestamp so static mount links survive + # timestamped graph lookups beside newer dynamic transforms. + for transform in message.transforms: + store.streams.tf.append(TFMessage(transform), ts=transform.ts) - return EdgeTAMImageSegmenter() - except ModuleNotFoundError as e: - raise ModuleNotFoundError( - "EdgeTAM requires the optional dependencies from dimos[misc]" - ) from e - return None + @rpc + def start(self) -> None: + if self.config.target_frame != "world": + raise ValueError("ObjectSceneRegistrationModule requires target_frame='world'") + + detector = DanDetector() + detector.start() + store = MemoryStore(max_size=self.config.max_image_observations) + self._create_streams(store) + with self._state_lock: + self._store = store + self._detector = detector + + self.color_image.subscribe(self._record_color_image) + self.depth_image.subscribe(self._record_depth_image) + self.camera_info.subscribe(self._record_camera_info) + self.tf.subscribe(self._record_tf) + index = detector.embed( + store, + live=True, + optical_frame=self.config.optical_frame, + world_frame=self.config.target_frame, + tf_tolerance=self.config.tf_tolerance_seconds, + ) + with self._state_lock: + self._index = index + super().start() @rpc def stop(self) -> None: - """Stop the module and clean up resources.""" - - with self._processing_lock: - if self._segmenter is not None: - self._segmenter.stop() - self._segmenter = None - if self._detector: - self._detector.stop() + with self._inference_lock: + with self._state_lock: + detector = self._detector + store = self._store self._detector = None - - self._object_db.clear() - self._latest_aligned_frames = None - - logger.info("ObjectSceneRegistrationModule stopped") + self._store = None + self._index = None + if detector is not None: + detector.stop() + if store is not None: + store.stop() super().stop() @rpc - def set_prompts( - self, - text: list[str] | None = None, - bboxes: NDArray[np.float64] | None = None, - ) -> None: - """Set prompts for detection. Provide either text or bboxes, not both.""" - with self._processing_lock: - self._set_prompts(text=text, bboxes=bboxes) - - def _set_prompts( - self, - text: list[str] | None = None, - bboxes: NDArray[np.float64] | None = None, - ) -> None: - if self._detector_backend in {"owlv2", "moondream"}: - if bboxes is not None: - raise ValueError(f"{self._detector_backend} supports text prompts only") - self._text_prompts = text or [] - elif self._detector is not None: - self._detector.set_prompts(text=text, bboxes=bboxes) - - @rpc - def scan_scene(self, text: list[str] | None = None) -> Detection3DArray: - """Run one serialized detection pass over the latest aligned RGB-D frame.""" - with self._processing_lock: - if text is not None: - self._set_prompts(text=text) - frames = self._latest_aligned_frames - if frames is None: - return to_detection3d_array([], frame_id=self._target_frame) - objects = self._scan_scene_objects(frames) - return to_detection3d_array( - objects, - frame_id=self._target_frame, - ts=frames[0].ts, + def localize_objects(self, prompts: list[str]) -> list[Localization | None]: + config = self.config + if not prompts or len(prompts) > config.max_prompts_per_request: + raise ValueError(f"Expected 1-{config.max_prompts_per_request} object prompts") + if any(not prompt or prompt.strip() != prompt for prompt in prompts): + raise ValueError("Object prompts must be non-empty and trimmed") + + with self._inference_lock: + with self._state_lock: + store = self._store + detector = self._detector + index = self._index + if store is None or detector is None or index is None: + raise RuntimeError("ObjectSceneRegistrationModule is not running") + if not store.streams.camera_info.exists(): + raise RuntimeError("Camera calibration is not available") + if not store.streams.color_image.exists(): + raise RuntimeError("No RGB observations are available") + + latest = store.streams.color_image.last() + snapshot = index.time_range( + latest.ts - config.memory_window_seconds, + latest.ts, + ).materialize() + if not snapshot.exists(): + raise RuntimeError("No indexed RGB observations are available") + + results = detector.localize( + store, + prompts, + index=snapshot, + require_pose=True, + world_frame=config.target_frame, + optical_frame=config.optical_frame, + tf_tolerance=config.tf_tolerance_seconds, + policy=LocalizePolicy( + candidate_floor=config.candidate_floor, + accept_score=config.accept_score, + refusal_margin=config.refusal_margin, + min_views=config.min_views, + ), ) - - def _scan_scene_objects(self, frames: tuple[Image, Image] | None = None) -> list[DetObject]: - """Process one frame and return only objects observed during this scan.""" - frames = frames or self._latest_aligned_frames - if frames is None: - return [] - return self._process_images(*frames) - - @rpc - def select_object(self, track_id: int) -> dict[str, Any] | None: - """Get object data by track_id and promote to permanent.""" - for obj in self._object_db.get_all_objects(): - if obj.track_id == track_id: - self._object_db.promote(obj.object_id) - return obj.to_dict() - return None - - @rpc - def get_object_track_ids(self) -> list[int]: - """Get track_ids of all permanent objects.""" - return [obj.track_id for obj in self._object_db.get_all_objects()] - - @rpc - def get_detected_objects(self) -> list[dict[str, Any]]: - """Get all detected objects with object_id (UUID) and name.""" - return [obj.agent_encode() for obj in self._object_db.get_all_objects()] - - @rpc - def get_object_pointcloud_by_name(self, name: str) -> PointCloud2 | None: - """Get pointcloud for an object by class name.""" - objects = self._object_db.find_by_name(name) - return objects[0].pointcloud if objects else None - - @rpc - def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | None: - """Get pointcloud for an object by its stable object_id (searches all objects).""" - obj = self._object_db.find_by_object_id(object_id) - if obj is None: - logger.warning(f"No object found with object_id='{object_id}'") - return None - pc = obj.pointcloud - num_points = len(pc.pointcloud.points) if pc else 0 - logger.info(f"Found object '{object_id}' ({obj.name}) with {num_points} points") - return pc - - def _get_object_mask(self, object_id: str) -> NDArray[np.uint8] | None: - """Get dilated mask for an object by ID.""" - import cv2 - - for obj in self._object_db.get_all_objects(): - if obj.object_id != object_id: - continue - if obj.mask is None: - return None - - mask = obj.mask.astype(np.uint8) - if mask.max() == 1: - mask = (mask * 255).astype(np.uint8) - - kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (15, 15)) - return cv2.dilate(mask, kernel).astype(np.uint8) - - return None - - @rpc - def get_full_scene_pointcloud( - self, - exclude_object_id: str | None = None, - depth_trunc: float = 2.0, - voxel_size: float = 0.01, - ) -> PointCloud2 | None: - """Get full scene pointcloud from depth, including table/surfaces for collision filtering.""" - import open3d as o3d # type: ignore[import-untyped] - - scene_snapshot = self._latest_scene_snapshot - if scene_snapshot is None or self._camera_info is None: - return None - - depth_image, camera_transform = scene_snapshot - depth_cv = depth_image.to_opencv() - h, w = depth_cv.shape[:2] - - # Zero out excluded object's depth - if exclude_object_id: - exclude_mask = self._get_object_mask(exclude_object_id) - if exclude_mask is not None: - depth_cv = depth_cv.copy() - depth_cv[exclude_mask > 0] = 0 - - # Build pointcloud from depth - fx, fy = self._camera_info.K[0], self._camera_info.K[4] - cx, cy = self._camera_info.K[2], self._camera_info.K[5] - intrinsic = o3d.camera.PinholeCameraIntrinsic(w, h, fx, fy, cx, cy) - - depth_o3d = o3d.geometry.Image(depth_cv.astype(np.float32)) - pcd = o3d.geometry.PointCloud.create_from_depth_image( - depth_o3d, intrinsic, depth_scale=1.0, depth_trunc=depth_trunc - ) - - if len(pcd.points) < 100: - return None - - pcd = pcd.voxel_down_sample(voxel_size) - - pc = PointCloud2( - pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - - if camera_transform is not None: - pc = pc.transform(camera_transform) - - return pc - - @skill - def detect(self, prompts: list[str]) -> str: - """Detect objects matching the given text prompts. - - Do NOT call this tool multiple times for one query. Pass all objects in a single call. - For example, to detect a cup and mouse, pass ["cup", "mouse"] in one call. - - Args: - prompts: Text descriptions of concrete object categories to detect. - - Returns: - str: Detected objects with their object_id (stable UUID) and name. - - Example: - detect(["person", "car", "dog"]) - detect(["cup"]) - """ - if not prompts: - return "No prompts provided." - with self._processing_lock: - if self._detector is None: - return "Detector not initialized." - self._set_prompts(text=prompts) - if self._detect_on_request: - detected = [obj.agent_encode() for obj in self._scan_scene_objects()] - else: - detected = None - if detected is None: - time.sleep(2.0) - detected = self.get_detected_objects() - if not detected: - return "No objects detected." - - obj_list = [f" - {obj['name']} (object_id='{obj['object_id']}')" for obj in detected] - return f"Detected {len(detected)} object(s):\n" + "\n".join(obj_list) - - @skill - def select(self, track_id: int) -> str: - """Select an object by track_id and promote it to permanent. - - Example: - select(5) - """ - result = self.select_object(track_id) - if result is None: - return f"No object found with track_id {track_id}." - return f"Selected object {track_id}: {result['name']}" - - def _on_aligned_frames(self, frames) -> None: # type: ignore[no-untyped-def] - color_msg, depth_msg = frames - self._latest_aligned_frames = (color_msg, depth_msg) - if self._detect_on_request: - return - with self._processing_lock: - self._process_images(color_msg, depth_msg) - - def _process_images(self, color_msg: Image, depth_msg: Image) -> list[DetObject]: - """Process synchronized color and depth images (runs in background thread).""" - if not self._detector or not self._camera_info: - return [] - - color_image = color_msg - # Convert depth to meters (float32) - depth_cv = depth_msg.to_opencv() - if depth_msg.format == ImageFormat.DEPTH16: - depth_cv = depth_cv.astype(np.float32) / 1000.0 - elif depth_cv.dtype != np.float32: - depth_cv = depth_cv.astype(np.float32) - depth_image = Image( - data=depth_cv, format=ImageFormat.DEPTH, frame_id=depth_msg.frame_id, ts=depth_msg.ts - ) - - camera_transform = None - if self._target_frame != color_image.frame_id: - camera_transform = self.tfbuffer.get( - self._target_frame, - color_image.frame_id, - color_image.ts, - 0.1, - forward_tolerance=0.2, - ) - - detections_2d: ImageDetections2D[Any] - if self._detector_backend == "owlv2": - if not self._text_prompts: - detections_2d = ImageDetections2D(color_image, []) - else: - detections_2d = self._detector.query_detections( - color_image, - self._text_prompts, - threshold=self._detector_confidence, - ) - elif self._detector_backend == "moondream": - detections_2d = ImageDetections2D(color_image, []) - for class_id, prompt in enumerate(self._text_prompts): - prompted = self._detector.query_detections(color_image, prompt) - for detection in prompted.detections: - # Moondream's per-query indices are not temporal track IDs. - detection.track_id = -1 - detection.class_id = class_id - detections_2d.detections.extend(prompted.detections) - else: - detections_2d = self._detector.process_image(color_image) - - if self._segmenter is not None: - detections_2d = self._segmenter.segment(detections_2d) - - detections_2d_msg = Detection2DArray( - detections_length=len(detections_2d.detections), - header=Header(color_image.ts, color_image.frame_id or ""), - detections=[det.to_ros_detection2d() for det in detections_2d.detections], - ) - self.detections_2d.publish(detections_2d_msg) - - # Process 3D detections - return self._process_3d_detections( - detections_2d, color_image, depth_image, camera_transform - ) - - def _process_3d_detections( - self, - detections_2d: ImageDetections2D[Any], - color_image: Image, - depth_image: Image, - camera_transform: Transform | None, - ) -> list[DetObject]: - """Convert 2D detections to 3D and publish.""" - if self._camera_info is None: - return [] - - if self._target_frame != color_image.frame_id and camera_transform is None: - logger.warning("Failed to lookup transform from camera frame to target frame") - return [] - - # Cache depth and transform together, only after the lookup succeeds. - self._latest_scene_snapshot = (depth_image, camera_transform) - - objects = Object.from_2d_to_list( - detections_2d=detections_2d, - color_image=color_image, - depth_image=depth_image, - camera_info=self._camera_info, - camera_transform=camera_transform, - max_distance=self._max_distance, - use_aabb=self._use_aabb, - max_obstacle_width=self._max_obstacle_width, - ) - - # Empty observations still advance pending-object expiry. - observed_objects = self._object_db.add_objects(objects) - if not objects: - return [] - - # Publish ALL permanent objects so downstream consumers get the full set, - # not just this frame's batch (which may be a subset of what's on the table). - all_permanent = self._object_db.get_objects() - - detections_3d = to_detection3d_array(all_permanent) - self.detections_3d.publish(detections_3d) - self.objects.publish(all_permanent) - - objects_for_pc = all_permanent - aggregated_pc = aggregate_pointclouds(objects_for_pc) - self.pointcloud.publish(aggregated_pc) - return observed_objects + if not isinstance(results, list) or len(results) != len(prompts): + raise RuntimeError("Detector returned an invalid batch result") + + validated: list[Localization | None] = [] + for result in results: + if result is None or result.reason: + validated.append(None) + continue + if result.frame_id != config.target_frame: + raise RuntimeError( + f"Localization frame mismatch: expected {config.target_frame!r}, " + f"got {result.frame_id!r}" + ) + if result.point_cloud is None: + raise RuntimeError("Localization is missing its point cloud") + if result.point_cloud.frame_id != config.target_frame: + raise RuntimeError( + "Localization point cloud frame mismatch: expected " + f"{config.target_frame!r}, got {result.point_cloud.frame_id!r}" + ) + if latest.ts - result.last_seen_timestamp > config.max_result_age_seconds: + validated.append(None) + continue + validated.append(result) + return validated diff --git a/dimos/perception/experimental/object_scene_registration_spec.py b/dimos/perception/experimental/object_scene_registration_spec.py index 5f0cbbf8e1..5e63c05d8b 100644 --- a/dimos/perception/experimental/object_scene_registration_spec.py +++ b/dimos/perception/experimental/object_scene_registration_spec.py @@ -14,19 +14,9 @@ from typing import Protocol -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray +from dimos.perception.memory.types import Localization from dimos.spec.utils import Spec class ObjectSceneRegistrationSpec(Spec, Protocol): - def set_prompts(self, text: list[str] | None = None) -> None: ... - def scan_scene(self, text: list[str] | None = None) -> Detection3DArray: ... - def get_object_pointcloud_by_name(self, name: str) -> PointCloud2 | None: ... - def get_object_pointcloud_by_object_id(self, object_id: str) -> PointCloud2 | None: ... - def get_full_scene_pointcloud( - self, - exclude_object_id: str | None = None, - depth_trunc: float = 2.0, - voxel_size: float = 0.01, - ) -> PointCloud2 | None: ... + def localize_objects(self, prompts: list[str]) -> list[Localization | None]: ... diff --git a/dimos/perception/experimental/test_object_scene_registration.py b/dimos/perception/experimental/test_object_scene_registration.py new file mode 100644 index 0000000000..206e4ed640 --- /dev/null +++ b/dimos/perception/experimental/test_object_scene_registration.py @@ -0,0 +1,189 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from collections.abc import Callable, Iterator +from types import SimpleNamespace +from typing import Any +from unittest.mock import MagicMock + +import pytest + +from dimos.memory.store.memory import MemoryStore +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.perception.experimental.object_scene_registration import ( + ObjectSceneRegistrationModule, +) +from dimos.perception.memory.types import Localization, LocalizePolicy + +ModuleFactory = Callable[..., ObjectSceneRegistrationModule] + + +def _localization( + *, + reason: str | None = None, + frame_id: str = "world", + cloud_frame: str = "world", + last_seen: float = 99.0, +) -> Localization: + cloud = MagicMock(spec=PointCloud2) + cloud.frame_id = cloud_frame + return Localization( + instance_id="query-0", + semantic_score=0.9, + identity_score=0.5, + ambiguity_margin=1.0, + position_world_xyz=(0.1, 0.2, 0.3), + orientation_world_xyzw=(0.0, 0.0, 0.0, 1.0), + frame_id=frame_id, + support=None, + pose_timestamp=last_seen, + geometry_timestamp=last_seen, + last_seen_timestamp=last_seen, + point_cloud=cloud, + cloud_mode="latest_visible", + coverage=0.5, + n_views=2, + reason=reason, + ) + + +@pytest.fixture +def module_factory() -> Iterator[ModuleFactory]: + modules: list[ObjectSceneRegistrationModule] = [] + + def create(**kwargs: Any) -> ObjectSceneRegistrationModule: + module = ObjectSceneRegistrationModule(**kwargs) + modules.append(module) + return module + + yield create + + for module in reversed(modules): + module.stop() + + +def _running_module( + results: list[Localization | None], module_factory: ModuleFactory +) -> tuple[ObjectSceneRegistrationModule, MagicMock]: + module = module_factory() + store = MagicMock() + store.streams.camera_info.exists.return_value = True + store.streams.color_image.exists.return_value = True + store.streams.color_image.last.return_value = SimpleNamespace(ts=100.0) + detector = MagicMock() + detector.localize.return_value = results + snapshot = MagicMock() + snapshot.exists.return_value = True + index = MagicMock() + index.time_range.return_value.materialize.return_value = snapshot + module._store = store + module._detector = detector + module._index = index + return module, detector + + +def test_localize_objects_batches_and_rejects_ambiguous_results( + module_factory: ModuleFactory, +) -> None: + accepted = _localization() + ambiguous = _localization(reason="ambiguous_between_coexisting_candidates") + module, detector = _running_module([accepted, ambiguous, None], module_factory) + + result = module.localize_objects(["cup", "bowl", "fork"]) + + assert result == [accepted, None, None] + detector.localize.assert_called_once_with( + module._store, + ["cup", "bowl", "fork"], + index=module._index.time_range.return_value.materialize.return_value, + require_pose=True, + world_frame="world", + optical_frame="camera_color_optical_frame", + tf_tolerance=0.12, + policy=LocalizePolicy(), + ) + + +@pytest.mark.parametrize("prompts", [[], [""], ["cup", " cup "]]) +def test_localize_objects_rejects_invalid_prompts( + prompts: list[str], module_factory: ModuleFactory +) -> None: + module, _ = _running_module([], module_factory) + + with pytest.raises(ValueError): + module.localize_objects(prompts) + + +def test_localize_objects_requires_ready_index(module_factory: ModuleFactory) -> None: + module = module_factory() + + with pytest.raises(RuntimeError, match="not running"): + module.localize_objects(["cup"]) + + +def test_localize_objects_rejects_invalid_batch_shape(module_factory: ModuleFactory) -> None: + module, detector = _running_module([], module_factory) + detector.localize.return_value = _localization() + + with pytest.raises(RuntimeError, match="invalid batch result"): + module.localize_objects(["cup"]) + + +@pytest.mark.parametrize( + ("localization", "message"), + [ + (_localization(frame_id="camera"), "Localization frame mismatch"), + (_localization(cloud_frame="camera"), "point cloud frame mismatch"), + ], +) +def test_localize_objects_rejects_frame_mismatch( + localization: Localization, message: str, module_factory: ModuleFactory +) -> None: + module, _ = _running_module([localization], module_factory) + + with pytest.raises(RuntimeError, match=message): + module.localize_objects(["cup"]) + + +def test_localize_objects_drops_stale_results(module_factory: ModuleFactory) -> None: + module, _ = _running_module([_localization(last_seen=60.0)], module_factory) + + assert module.localize_objects(["cup"]) == [None] + + +def test_record_tf_splits_messages_for_timestamped_lookup( + module_factory: ModuleFactory, +) -> None: + module = module_factory(max_tf_observations=2) + store = MemoryStore(max_size=10) + module._store = store + module._create_streams(store) + first = Transform(ts=1.0, frame_id="world", child_frame_id="link7") + second = Transform(ts=2.0, frame_id="link7", child_frame_id="camera_link") + third = Transform(ts=3.0, frame_id="camera_link", child_frame_id="camera_color_frame") + + module._record_tf(TFMessage(first, second, third)) + + observations = store.streams.tf.to_list() + assert [observation.ts for observation in observations] == [2.0, 3.0] + assert [len(observation.data.transforms) for observation in observations] == [1, 1] + + +def test_non_world_target_is_rejected_before_model_start(module_factory: ModuleFactory) -> None: + module = module_factory(target_frame="base_link") + + with pytest.raises(ValueError, match="target_frame='world'"): + module.start() diff --git a/dimos/perception/experimental/test_object_scene_registration_temporal.py b/dimos/perception/experimental/test_object_scene_registration_temporal.py deleted file mode 100644 index 82f14417f9..0000000000 --- a/dimos/perception/experimental/test_object_scene_registration_temporal.py +++ /dev/null @@ -1,523 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Iterator -import sys -from threading import Event, Thread -from typing import Any -from unittest.mock import ANY, MagicMock, call - -import numpy as np -import pytest - -from dimos.msgs.sensor_msgs.Image import Image, ImageFormat -from dimos.msgs.vision_msgs.Detection3DArray import Detection3DArray -from dimos.perception.detection.type.detection2d.imageDetections2D import ImageDetections2D -from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule -from dimos.perception.experimental.objectDB import ObjectDB - - -class _FakeTF: - def __init__(self, result: Any) -> None: - self.result = result - self.calls: list[tuple[Any, ...]] = [] - - def get(self, *args: Any, **kwargs: Any) -> Any: - self.calls.append((args, kwargs)) - return self.result - - def dispose(self) -> None: - pass - - -def _image(timestamp: float) -> Image: - return Image( - data=np.ones((2, 2), dtype=np.float32), - format=ImageFormat.DEPTH, - frame_id="camera", - ts=timestamp, - ) - - -@pytest.fixture -def module() -> Iterator[ObjectSceneRegistrationModule]: - module = ObjectSceneRegistrationModule(target_frame="map") - module._camera_info = MagicMock(K=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0]) - module._latest_scene_snapshot = None - yield module - module.stop() - - -def test_transform_is_captured_before_slow_detector( - monkeypatch: Any, module: ObjectSceneRegistrationModule -) -> None: - now = [0.0] - transform = MagicMock(name="fresh_transform") - - class _ExpiringTF(_FakeTF): - def get(self, *args: Any, **kwargs: Any) -> Any: - super().get(*args, **kwargs) - return transform if now[0] <= 10.0 else None - - tf = _ExpiringTF(None) - module._tf = tf # type: ignore[assignment] - module._detector_backend = "owlv2" - module._detector = MagicMock() - module._text_prompts = ["cup"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=12.5, - ) - detections = ImageDetections2D(color, []) - - def slow_detection(*_args: Any, **_kwargs: Any) -> ImageDetections2D: - now[0] = 11.0 - return detections - - module._detector.query_detections.side_effect = slow_detection - process_3d = MagicMock() - monkeypatch.setattr(module, "_process_3d_detections", process_3d) - - module._process_images(color, _image(12.5)) - - assert tf.calls == [(("map", "camera", 12.5, 0.1), {"forward_tolerance": 0.2})] - process_3d.assert_called_once_with(detections, color, ANY, transform) - - -def test_failed_lookup_does_not_retry_without_time_or_replace_coherent_cache( - monkeypatch: Any, module: ObjectSceneRegistrationModule -) -> None: - old_transform = MagicMock(name="old_transform") - warning = MagicMock() - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.logger.warning", warning - ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.Object.from_2d_to_list", - lambda **_: [], - ) - - old_depth = _image(1.0) - ObjectSceneRegistrationModule._process_3d_detections( - module, - MagicMock(spec=ImageDetections2D), - old_depth, - old_depth, - old_transform, - ) - new_depth = _image(2.0) - ObjectSceneRegistrationModule._process_3d_detections( - module, - MagicMock(spec=ImageDetections2D), - new_depth, - new_depth, - None, - ) - - assert module._latest_scene_snapshot == (old_depth, old_transform) - warning.assert_called_once_with("Failed to lookup transform from camera frame to target frame") - - -def test_full_scene_pointcloud_uses_one_coherent_scene_snapshot( - monkeypatch: Any, module: ObjectSceneRegistrationModule -) -> None: - depth = _image(3.0) - transform = MagicMock(name="transform") - module._tf = _FakeTF(transform) # type: ignore[assignment] - module._latest_scene_snapshot = (depth, transform) - - class _PointCloud: - points = list(range(100)) - - def voxel_down_sample(self, voxel_size: float) -> _PointCloud: - return self - - pointcloud = _PointCloud() - fake_o3d = MagicMock() - fake_o3d.camera.PinholeCameraIntrinsic.return_value = MagicMock() - fake_o3d.geometry.Image.return_value = MagicMock() - fake_o3d.geometry.PointCloud.create_from_depth_image.return_value = pointcloud - # open3d is imported inside the method under test, so swap the module itself - monkeypatch.setitem(sys.modules, "open3d", fake_o3d) - - result = MagicMock() - result.transform.side_effect = lambda used_transform: ( - result if used_transform is transform else pytest.fail("mixed scene snapshot") - ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.PointCloud2", - lambda *_args, **_kwargs: result, - ) - - module.get_full_scene_pointcloud() - result.transform.assert_called_once_with(transform) - - -def test_owlv2_queries_configured_prompts(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( - target_frame="camera", detector_backend="owlv2", detector_confidence=0.07 - ) - module._camera_info = MagicMock() - module._detector = MagicMock() - module._text_prompts = ["mug"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, - ) - detections = ImageDetections2D(color, []) - module._detector.query_detections.return_value = detections - process_3d = MagicMock() - monkeypatch.setattr(module, "_process_3d_detections", process_3d) - - module._process_images(color, _image(4.0)) - - module._detector.query_detections.assert_called_once_with(color, ["mug"], threshold=0.07) - process_3d.assert_called_once_with(detections, color, ANY, None) - module.stop() - - -def test_owlv2_yolo_constructs_box_prompt_segmenter(mocker: Any) -> None: - module = ObjectSceneRegistrationModule( - detector_backend="owlv2", - segmentation_backend="yolo", - segmentation_confidence=0.04, - ) - segmenter = MagicMock() - segmenter_factory = mocker.patch( - "dimos.perception.experimental.object_scene_registration.YoloeBoxSegmenter", - return_value=segmenter, - ) - - try: - created = module._create_segmenter() - finally: - module.stop() - - assert created is segmenter - segmenter_factory.assert_called_once_with(confidence=0.04) - - -def test_moondream_queries_each_configured_prompt(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera", detector_backend="moondream") - module._camera_info = MagicMock() - module._detector = MagicMock() - module._text_prompts = ["cup", "bottle"] - module.detections_2d = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, - ) - cup = MagicMock(track_id=0, class_id=-1) - bottle = MagicMock(track_id=0, class_id=-1) - module._detector.query_detections.side_effect = [ - ImageDetections2D(color, [cup]), - ImageDetections2D(color, [bottle]), - ] - process_3d = MagicMock() - monkeypatch.setattr(module, "_process_3d_detections", process_3d) - - module._process_images(color, _image(4.0)) - - assert module._detector.query_detections.call_args_list == [ - call(color, "cup"), - call(color, "bottle"), - ] - combined = process_3d.call_args.args[0] - assert combined.detections == [cup, bottle] - assert (cup.track_id, cup.class_id) == (-1, 0) - assert (bottle.track_id, bottle.class_id) == (-1, 1) - module.stop() - - -def test_edgetam_refines_detector_output(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera", segmentation_backend="edgetam") - module._camera_info = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, - ) - raw_detections = ImageDetections2D(color, []) - segmented_detections = ImageDetections2D(color, []) - module._segmenter = MagicMock() - module._segmenter.segment.return_value = segmented_detections - module._detector = MagicMock() - module._detector.process_image.return_value = raw_detections - module.detections_2d = MagicMock() - process_3d = MagicMock() - monkeypatch.setattr(module, "_process_3d_detections", process_3d) - - module._process_images(color, _image(4.0)) - - module._segmenter.segment.assert_called_once_with(raw_detections) - process_3d.assert_called_once_with(segmented_detections, color, ANY, None) - module.stop() - - -def test_request_driven_scan_processes_latest_aligned_frame(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( - target_frame="camera", detector_backend="owlv2", detect_on_request=True - ) - module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, - ) - depth = _image(4.0) - module._on_aligned_frames((color, depth)) - output = MagicMock() - result = MagicMock(spec=Detection3DArray) - - def process_images(got_color: Image, got_depth: Image) -> list[MagicMock]: - assert (got_color, got_depth) == (color, depth) - return [output] - - monkeypatch.setattr(module, "_process_images", process_images) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.to_detection3d_array", - lambda objects, **kwargs: result, - ) - - assert module.scan_scene(text=["mug"]) is result - assert module._text_prompts == ["mug"] - module.stop() - - -def test_request_driven_detect_triggers_scan(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(detector_backend="owlv2", detect_on_request=True) - module._detector = MagicMock() - current_object = MagicMock() - current_object.agent_encode.return_value = {"name": "mug", "object_id": "current"} - scan_objects = MagicMock(return_value=[current_object]) - monkeypatch.setattr(module, "_scan_scene_objects", scan_objects) - monkeypatch.setattr(module, "get_detected_objects", lambda: pytest.fail("read stale database")) - - assert module.detect(["mug"]) == "Detected 1 object(s):\n - mug (object_id='current')" - assert module._text_prompts == ["mug"] - scan_objects.assert_called_once_with() - module.stop() - - -def test_scan_output_includes_pending_objects(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule(target_frame="camera") - module._camera_info = MagicMock() - module._object_db = MagicMock() - pending = MagicMock() - permanent = MagicMock() - module._object_db.get_all_objects.return_value = [pending, permanent] - module._object_db.get_objects.return_value = [permanent] - module._object_db.add_objects.return_value = [pending] - module.detections_3d = MagicMock() - module.objects = MagicMock() - module.pointcloud = MagicMock() - converted_objects = [pending] - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.Object.from_2d_to_list", - lambda **_: converted_objects, - ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.to_detection3d_array", - MagicMock(), - ) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.aggregate_pointclouds", - MagicMock(), - ) - - observed = ObjectSceneRegistrationModule._process_3d_detections( - module, - MagicMock(spec=ImageDetections2D), - _image(4.0), - _image(4.0), - None, - ) - - assert observed == [pending] - - converted_objects.clear() - module._object_db.add_objects.reset_mock() - observed = ObjectSceneRegistrationModule._process_3d_detections( - module, - MagicMock(spec=ImageDetections2D), - _image(5.0), - _image(5.0), - None, - ) - - assert observed == [] - module._object_db.add_objects.assert_called_once_with([]) - module.stop() - - -def test_concurrent_request_scans_do_not_overlap(monkeypatch: Any) -> None: - module = ObjectSceneRegistrationModule( - target_frame="camera", detector_backend="owlv2", detect_on_request=True - ) - module._object_db = MagicMock() - color = Image( - data=np.zeros((2, 2, 3), dtype=np.uint8), - format=ImageFormat.BGR, - frame_id="camera", - ts=4.0, - ) - module._on_aligned_frames((color, _image(4.0))) - first_started = Event() - release_first = Event() - second_started = Event() - seen_prompts: list[list[str]] = [] - - def process_images(_: Image, __: Image) -> list[MagicMock]: - seen_prompts.append(list(module._text_prompts)) - if len(seen_prompts) == 1: - first_started.set() - assert release_first.wait(timeout=1.0) - else: - second_started.set() - return [] - - monkeypatch.setattr(module, "_process_images", process_images) - monkeypatch.setattr( - "dimos.perception.experimental.object_scene_registration.to_detection3d_array", - MagicMock(), - ) - first = Thread(target=lambda: module.scan_scene(text=["cup"])) - second = Thread(target=lambda: module.scan_scene(text=["mug"])) - first.start() - assert first_started.wait(timeout=1.0) - second.start() - assert not second_started.wait(timeout=0.05) - release_first.set() - first.join(timeout=1.0) - second.join(timeout=1.0) - - assert not first.is_alive() - assert not second.is_alive() - assert seen_prompts == [["cup"], ["mug"]] - module.stop() - - -def test_object_db_uses_wall_clock_for_pending_ttl(monkeypatch: Any) -> None: - object_db = ObjectDB(pending_ttl_s=5.0) - detected = MagicMock() - detected.object_id = "stable-id" - detected.track_id = -1 - detected.ts = 4.0 # Hardware timestamps are relative to camera boot. - detected.last_seen_ts = None - detected.center = None - detected.detections_count = 1 - now = [1000.0] - monkeypatch.setattr("dimos.perception.experimental.objectDB.time.time", lambda: now[0]) - - object_db.add_objects([detected]) - assert detected.last_seen_ts == 1000.0 - now[0] = 1004.0 - object_db.add_objects([]) - assert object_db.find_by_object_id("stable-id") is detected - now[0] = 1006.0 - object_db.add_objects([]) - assert object_db.find_by_object_id("stable-id") is None - - -def test_object_db_counts_each_source_frame_once(monkeypatch: Any) -> None: - object_db = ObjectDB(min_detections_for_permanent=10) - now = [1000.0] - monkeypatch.setattr("dimos.perception.experimental.objectDB.time.time", lambda: now[0]) - - first = MagicMock( - object_id="first-id", - track_id=-1, - ts=4.0, - last_seen_ts=None, - detections_count=1, - ) - first.center = MagicMock() - duplicate = MagicMock(object_id="duplicate-id", track_id=-1, ts=4.0) - duplicate.center = MagicMock() - duplicate.center.distance.return_value = 0.0 - - observed = object_db.add_objects([first, duplicate]) - - assert observed == [first] - first.update_object.assert_not_called() - assert first.last_seen_ts == 1000.0 - - newer = MagicMock(object_id="newer-id", track_id=-1, ts=5.0) - newer.center = MagicMock() - newer.center.distance.return_value = 0.0 - first.update_object.side_effect = lambda _: setattr(first, "detections_count", 2) - now[0] = 1001.0 - - assert object_db.add_objects([newer]) == [first] - first.update_object.assert_called_once_with(newer) - assert first.last_seen_ts == 1001.0 - - -def test_object_db_promotes_a_first_sighting_when_threshold_is_one() -> None: - object_db = ObjectDB(min_detections_for_permanent=1) - detected = MagicMock( - object_id="first-id", - track_id=-1, - ts=1.0, - last_seen_ts=None, - detections_count=1, - name="cup", - ) - detected.center = None - - object_db.add_objects([detected]) - - assert object_db.get_objects() == [detected] - assert object_db.get_stats() == { - "pending_count": 0, - "permanent_count": 1, - "total_count": 1, - } - - -def test_object_db_keeps_first_sighting_pending_above_threshold() -> None: - object_db = ObjectDB(min_detections_for_permanent=2) - detected = MagicMock( - object_id="first-id", - track_id=7, - ts=1.0, - last_seen_ts=None, - detections_count=1, - name="cup", - ) - detected.center = None - newer = MagicMock(object_id="newer-id", track_id=7, ts=2.0) - newer.center = None - detected.update_object.side_effect = lambda _: setattr(detected, "detections_count", 2) - - object_db.add_objects([detected]) - - assert object_db.get_objects() == [] - assert object_db.find_by_object_id("first-id") is detected - - object_db.add_objects([newer]) - - assert object_db.get_objects() == [detected] diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 881abbba18..3746318308 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -51,7 +51,6 @@ "demo-google-maps-skill": "dimos.agents.skills.demo_google_maps_skill:demo_google_maps_skill", "demo-gps-nav": "dimos.agents.skills.demo_gps_nav:demo_gps_nav", "demo-mcp-stress-test": "dimos.core.demos.stress_test_blueprint:demo_mcp_stress_test", - "demo-object-scene-registration": "dimos.perception.experimental.demo_object_scene_registration:demo_object_scene_registration", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "demo-virtual-mid360-fastlio": "dimos.hardware.sensors.lidar.virtual_mid360.blueprints:demo_virtual_mid360_fastlio", @@ -219,7 +218,6 @@ "google-maps-skill-container": "dimos.agents.skills.google_maps_skill_container.GoogleMapsSkillContainer", "gps-nav-skill-container": "dimos.agents.skills.gps_nav_skill.GpsNavSkillContainer", "grasp-gen-x-module": "dimos.manipulation.grasping.grasp_gen_x.GraspGenXModule", - "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule", "hand-teleop-module": "dimos.teleop.quest.quest_extensions.HandTeleopModule", "heuristic-grasp-module": "dimos.manipulation.grasping.heuristic_grasp.HeuristicGraspModule", diff --git a/dimos/robot/manipulators/common/agent_prompts.py b/dimos/robot/manipulators/common/agent_prompts.py index b280f36105..08e7c76141 100644 --- a/dimos/robot/manipulators/common/agent_prompts.py +++ b/dimos/robot/manipulators/common/agent_prompts.py @@ -50,12 +50,12 @@ # Skills ## Perception -- **scan_objects**: Scan the latest aligned RGB-D frame for one or more object prompts. \ -Use before picking or after a failed grasp. Its result includes object IDs for the latest scan. +- **scan_objects**: Localize one or more object prompts from recent RGB-D history. \ +Use before picking or after a failed grasp. Its result includes integer selections for that scan. ## Pick & Place -- **pick_object **: Generate ranked grasp proposals and automatically execute the \ -top proposal. Use an exact object ID from the latest scan_objects result. +- **pick_object **: Generate ranked grasp proposals and automatically execute the \ +top proposal. Use an exact integer selection from the latest scan_objects result. - **place_at **: Place the verified held object at explicit world-frame \ coordinates. @@ -75,11 +75,11 @@ # Pick Workflow 1. Call **scan_objects** with all requested object prompts. -2. Call **pick_object** with the exact object ID returned by the scan. +2. Call **pick_object** with the exact integer selection returned by the scan. 3. Call **place_at** only after a successful pick. # Rules -- Use an exact object ID from the latest scan output. Do NOT select by name. +- Use an exact integer selection from the latest scan output. Do NOT select by name. - "place it at [coords]" → **place_at** after a successful **pick_object**. - "bring it back" → pick, then **go_init**. Do NOT place randomly. - "bring it to me" / "hand it over" → pick, then move toward user (≈ X=0, Y=0.5). diff --git a/dimos/robot/manipulators/xarm/blueprints/perception.py b/dimos/robot/manipulators/xarm/blueprints/perception.py index 8a59c23eee..cbe178283b 100644 --- a/dimos/robot/manipulators/xarm/blueprints/perception.py +++ b/dimos/robot/manipulators/xarm/blueprints/perception.py @@ -29,13 +29,22 @@ from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.perception.experimental.object_scene_registration import ObjectSceneRegistrationModule +from dimos.protocol.tf.static_tf_publisher import StaticTfPublisher from dimos.robot.manipulators.xarm.config import make_xarm7_model_config XARM_PERCEPTION_CAMERA_TRANSFORM = Transform( translation=Vector3(x=0.06693724, y=-0.0309563, z=0.00691482), rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw + frame_id="link7", + child_frame_id="camera_link", ) + +class _XArmCameraMountTfPublisher(StaticTfPublisher): + def transforms(self) -> list[Transform]: + return [XARM_PERCEPTION_CAMERA_TRANSFORM] + + xarm_perception = autoconnect( ManipulationModule.blueprint( model=make_xarm7_model_config( @@ -54,18 +63,10 @@ ManipulationSkills.blueprint(), PickAndPlaceModule.blueprint(planning_frame="world"), HeuristicGraspModule.blueprint(), - # TODO: tf tree is broken here; RealSenseCamera no longer publishes its mount - # edge, so camera_link needs a parent (e.g. from the arm) to resolve into world. + _XArmCameraMountTfPublisher.blueprint(), RealSenseCamera.blueprint(), ObjectSceneRegistrationModule.blueprint( target_frame="world", - detector_backend="moondream", - segmentation_backend="edgetam", - detect_on_request=True, - distance_threshold=0.08, - min_detections_for_permanent=3, - max_distance=1.0, - use_aabb=True, - max_obstacle_width=0.06, + optical_frame="camera_color_optical_frame", ), ).global_config(n_workers=4) diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 1d576733ee..9d005f40c7 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -67,9 +67,7 @@ MujocoSimModule.blueprint(**make_xarm7_sim_module_kwargs(XARM7_SIM_PATH)), ObjectSceneRegistrationModule.blueprint( target_frame="world", - detector_backend="moondream", - segmentation_backend="edgetam", - detect_on_request=True, + optical_frame="wrist_camera_color_optical_frame", ), coordinator( hardware=[_xarm7_sim_hw], @@ -108,18 +106,12 @@ ), ObjectSceneRegistrationModule.blueprint( target_frame="world", - detector_backend="owlv2", - # OWLv2 is box-only; YOLO-E visual prompts refine its boxes into masks. - segmentation_backend="yolo", # Synthetic MuJoCo renders score far below natural images. - detector_confidence=0.07, - segmentation_confidence=0.05, - # Keep adjacent tabletop targets distinct instead of merging by label. - distance_threshold=0.05, - detect_on_request=True, - # The obstacle stream contains permanent objects only; one explicit - # room scan must therefore promote its first sightings immediately. - min_detections_for_permanent=1, + candidate_floor=0.07, + accept_score=0.07, + # This deterministic room scan intentionally uses one fixed overview. + min_views=1, + optical_frame="wrist_camera_color_optical_frame", ), coordinator( hardware=[_xarm_room_sim_hw], diff --git a/dimos/robot/manipulators/xarm/blueprints/test_perception.py b/dimos/robot/manipulators/xarm/blueprints/test_perception.py new file mode 100644 index 0000000000..9db3d76c65 --- /dev/null +++ b/dimos/robot/manipulators/xarm/blueprints/test_perception.py @@ -0,0 +1,62 @@ +# 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.core.coordination.blueprint_config.parser import BlueprintConfigParser +from dimos.perception.experimental.object_scene_registration import ( + ObjectSceneRegistrationConfig, + ObjectSceneRegistrationModule, +) +from dimos.robot.manipulators.xarm.blueprints.perception import ( + XARM_PERCEPTION_CAMERA_TRANSFORM, + xarm_perception, +) +from dimos.robot.manipulators.xarm.blueprints.simulation import ( + xarm_perception_sim, + xarm_room_sim, +) + + +def _osr_config(blueprint) -> ObjectSceneRegistrationConfig: # type: ignore[no-untyped-def] + atom = next( + atom + for atom in blueprint.active_blueprints + if issubclass(atom.module, ObjectSceneRegistrationModule) + ) + parsed = BlueprintConfigParser(blueprint).parse(environ={}) + return ObjectSceneRegistrationConfig.model_validate(parsed.module_kwargs(atom.name)) + + +def test_real_camera_mount_connects_link7_to_camera_link() -> None: + assert XARM_PERCEPTION_CAMERA_TRANSFORM.frame_id == "link7" + assert XARM_PERCEPTION_CAMERA_TRANSFORM.child_frame_id == "camera_link" + + +def test_real_and_simulation_use_their_optical_frames() -> None: + assert _osr_config(xarm_perception).optical_frame == "camera_color_optical_frame" + assert _osr_config(xarm_perception_sim).optical_frame == "wrist_camera_color_optical_frame" + + +def test_room_sim_uses_single_view_synthetic_thresholds() -> None: + atom = next( + atom + for atom in xarm_room_sim.active_blueprints + if issubclass(atom.module, ObjectSceneRegistrationModule) + ) + # Parsing the complete room blueprint resolves its LFS-backed MuJoCo path. + config = ObjectSceneRegistrationConfig.model_validate(atom.kwargs) + + assert config.optical_frame == "wrist_camera_color_optical_frame" + assert config.candidate_floor == 0.07 + assert config.accept_score == 0.07 + assert config.min_views == 1 diff --git a/docs/capabilities/manipulation/xarm-room-sim.md b/docs/capabilities/manipulation/xarm-room-sim.md index c88a2bc20b..1964e2b32f 100644 --- a/docs/capabilities/manipulation/xarm-room-sim.md +++ b/docs/capabilities/manipulation/xarm-room-sim.md @@ -1,8 +1,8 @@ # xArm Room Simulation `xarm-room-sim` launches the complete headless room demo: xArm7 MuJoCo -simulation, wrist-camera OWL-ViT scene registration, perception-backed planner -obstacles, composed pick-and-place, and the control coordinator. +simulation, wrist-camera Dan localization, composed pick-and-place, and the +control coordinator. ```bash MUJOCO_GL=egl LIBGL_ALWAYS_SOFTWARE=true MESA_LOADER_DRIVER_OVERRIDE=llvmpipe \ @@ -11,9 +11,9 @@ MUJOCO_GL=egl LIBGL_ALWAYS_SOFTWARE=true MESA_LOADER_DRIVER_OVERRIDE=llvmpipe \ The blueprint disables the MuJoCo and manipulation viewers itself. On a host where `/dev/dri` must be hidden from Mesa, run the same command in the team's -existing `/dev/dri`-masked mount namespace. CPU OWL-ViT inference takes about -11 seconds per prompt/frame on the validation host, so allow the scan to -finish rather than issuing another scan concurrently. +existing `/dev/dri`-masked mount namespace. Dan's EdgeTAM segmenter requires a +CUDA or MPS device. Allow each scan to finish rather than issuing another scan +concurrently. The scene is an enclosed 2.6 m by 3.0 m room. The xArm stands on a 12 cm base pedestal at the room origin; its planning model uses that base pose directly. @@ -42,7 +42,7 @@ In a second terminal, connect to the running blueprint: dimos shell ``` -Then run this complete scan and obstacle-inspection sequence: +Then run a complete scan: ```python skip from dimos.robot.manipulators.xarm.blueprints.simulation import XARM_ROOM_PROMPTS @@ -50,12 +50,9 @@ from dimos.robot.manipulators.xarm.blueprints.simulation import XARM_ROOM_PROMPT app.ManipulationSkills.go_init() scan = app.PickAndPlaceModule.scan_objects(XARM_ROOM_PROMPTS) print(scan) - -print(app.ObjectSceneRegistrationModule.get_detected_objects()) -print(app.ManipulationModule.refresh_obstacles()) -print(app.ManipulationModule.get_obstacles()) ``` Wait for `scan_objects` to finish before issuing another scan. The prompt set includes a `green ring` fallback because the tape loses its category silhouette -in the wrist camera's top-down view. +in the wrist camera's top-down view. Pass an integer `selection` from the scan +result to `pick_object`.