From 825c6dda922f46309dedca7a6362861bf2e1aeae Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Thu, 11 Sep 2025 23:38:16 +0300 Subject: [PATCH 1/2] pSHMTransport to LCMTransport --- dimos/robot/unitree_webrtc/unitree_go2.py | 19 +++++-------------- 1 file changed, 5 insertions(+), 14 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 378dc01b99..99bb38b589 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -401,9 +401,7 @@ def _deploy_connection(self): self.connection.lidar.transport = core.LCMTransport("/lidar", LidarMessage) self.connection.odom.transport = core.LCMTransport("/odom", PoseStamped) - self.connection.video.transport = core.pSHMTransport( - "/go2/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE - ) + self.connection.video.transport = core.LCMTransport("/go2/color_image", Image) self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) self.connection.camera_info.transport = core.LCMTransport("/go2/camera_info", CameraInfo) self.connection.camera_pose.transport = core.LCMTransport("/go2/camera_pose", PoseStamped) @@ -478,7 +476,7 @@ def _deploy_visualization(self): self.websocket_vis.path.connect(self.global_planner.path) self.websocket_vis.global_costmap.connect(self.mapper.global_costmap) - self.foxglove_bridge = FoxgloveBridge(shm_channels=["/go2/color_image#sensor_msgs.Image"]) + self.foxglove_bridge = FoxgloveBridge() def _deploy_perception(self): """Deploy and configure perception modules.""" @@ -491,10 +489,7 @@ def _deploy_perception(self): output_dir=self.spatial_memory_dir, ) - color_image_default_capacity = 1920 * 1080 * 4 - self.spatial_memory_module.video.transport = core.pSHMTransport( - "/go2/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE - ) + self.spatial_memory_module.video.transport = core.LCMTransport("/go2/color_image", Image) self.spatial_memory_module.odom.transport = core.LCMTransport( "/go2/camera_pose", PoseStamped ) @@ -526,12 +521,8 @@ def _deploy_camera(self): self.depth_module = self.dimos.deploy(DepthModule, gt_depth_scale=gt_depth_scale) # Set up transports - self.depth_module.color_image.transport = core.pSHMTransport( - "/go2/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE - ) - self.depth_module.depth_image.transport = core.pSHMTransport( - "/go2/depth_image", default_capacity=DEFAULT_CAPACITY_DEPTH_IMAGE - ) + self.depth_module.color_image.transport = core.LCMTransport("/go2/color_image", Image) + self.depth_module.depth_image.transport = core.LCMTransport("/go2/depth_image", Image) self.depth_module.camera_info.transport = core.LCMTransport("/go2/camera_info", CameraInfo) logger.info("Camera module deployed and connected") From a60f72d8b773242fa3371fe4d577c1ed0beb2eb7 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 12 Sep 2025 01:33:39 +0300 Subject: [PATCH 2/2] fix Detection2DArray --- dimos/agents2/test_mock_agent.py | 5 +++-- .../visual_servoing/detection3d.py | 3 +-- .../visual_servoing/manipulation_module.py | 2 +- dimos/manipulation/visual_servoing/pbvs.py | 3 ++- dimos/msgs/vision_msgs/BoundingBox2DArray.py | 19 +++++++++++++++++++ dimos/msgs/vision_msgs/BoundingBox3DArray.py | 19 +++++++++++++++++++ dimos/msgs/vision_msgs/Detection2DArray.py | 19 +++++++++++++++++++ dimos/msgs/vision_msgs/Detection3DArray.py | 19 +++++++++++++++++++ dimos/msgs/vision_msgs/__init__.py | 6 ++++++ dimos/perception/detection2d/__init__.py | 2 +- dimos/perception/detection2d/module.py | 14 +++++--------- dimos/perception/object_tracker.py | 3 +-- .../unitree_webrtc/modular/ivan_unitree.py | 5 +++-- dimos/robot/unitree_webrtc/unitree_go2.py | 2 +- 14 files changed, 100 insertions(+), 21 deletions(-) create mode 100644 dimos/msgs/vision_msgs/BoundingBox2DArray.py create mode 100644 dimos/msgs/vision_msgs/BoundingBox3DArray.py create mode 100644 dimos/msgs/vision_msgs/Detection2DArray.py create mode 100644 dimos/msgs/vision_msgs/Detection3DArray.py create mode 100644 dimos/msgs/vision_msgs/__init__.py diff --git a/dimos/agents2/test_mock_agent.py b/dimos/agents2/test_mock_agent.py index 1a6adaf075..1f42877776 100644 --- a/dimos/agents2/test_mock_agent.py +++ b/dimos/agents2/test_mock_agent.py @@ -27,7 +27,8 @@ from dimos.msgs.foxglove_msgs import ImageAnnotations from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Vector3 from dimos.msgs.sensor_msgs import Image -from dimos.perception.detection2d import Detect2DModule, Detection2DArrayFix +from dimos.msgs.vision_msgs import Detection2DArray +from dimos.perception.detection2d import Detect2DModule from dimos.protocol.skill.test_coordinator import SkillContainerTest from dimos.robot.unitree_webrtc.modular.connection_module import ConnectionModule from dimos.robot.unitree_webrtc.type.lidar import LidarMessage @@ -166,7 +167,7 @@ async def test_tool_call_implicit_detections(): robot_connection.start() detect2d = dimos.deploy(Detect2DModule) - detect2d.detections.transport = LCMTransport("/detections", Detection2DArrayFix) + detect2d.detections.transport = LCMTransport("/detections", Detection2DArray) detect2d.annotations.transport = LCMTransport("/annotations", ImageAnnotations) detect2d.image.connect(robot_connection.video) detect2d.start() diff --git a/dimos/manipulation/visual_servoing/detection3d.py b/dimos/manipulation/visual_servoing/detection3d.py index 5fcc1451b6..0b78f3518c 100644 --- a/dimos/manipulation/visual_servoing/detection3d.py +++ b/dimos/manipulation/visual_servoing/detection3d.py @@ -28,14 +28,13 @@ from dimos.msgs.geometry_msgs import Pose, Vector3, Quaternion from dimos.msgs.std_msgs import Header +from dimos.msgs.vision_msgs import Detection2DArray, Detection3DArray from dimos_lcm.vision_msgs import ( Detection3D, - Detection3DArray, BoundingBox3D, ObjectHypothesisWithPose, ObjectHypothesis, Detection2D, - Detection2DArray, BoundingBox2D, Pose2D, Point2D, diff --git a/dimos/manipulation/visual_servoing/manipulation_module.py b/dimos/manipulation/visual_servoing/manipulation_module.py index eda3daa557..a3fe0a17f9 100644 --- a/dimos/manipulation/visual_servoing/manipulation_module.py +++ b/dimos/manipulation/visual_servoing/manipulation_module.py @@ -29,8 +29,8 @@ from dimos.core import Module, In, Out, rpc from dimos.msgs.sensor_msgs import Image from dimos.msgs.geometry_msgs import Vector3, Pose, Quaternion +from dimos.msgs.vision_msgs import Detection2DArray, Detection3DArray from dimos_lcm.sensor_msgs import CameraInfo -from dimos_lcm.vision_msgs import Detection3DArray, Detection2DArray from dimos.hardware.piper_arm import PiperArm from dimos.manipulation.visual_servoing.detection3d import Detection3DProcessor diff --git a/dimos/manipulation/visual_servoing/pbvs.py b/dimos/manipulation/visual_servoing/pbvs.py index 77b4103104..a8f5ce5621 100644 --- a/dimos/manipulation/visual_servoing/pbvs.py +++ b/dimos/manipulation/visual_servoing/pbvs.py @@ -22,7 +22,8 @@ from collections import deque from scipy.spatial.transform import Rotation as R from dimos.msgs.geometry_msgs import Pose, Vector3, Quaternion -from dimos_lcm.vision_msgs import Detection3D, Detection3DArray +from dimos.msgs.vision_msgs import Detection3DArray +from dimos_lcm.vision_msgs import Detection3D from dimos.utils.logging_config import setup_logger from dimos.manipulation.visual_servoing.utils import ( update_target_grasp_pose, diff --git a/dimos/msgs/vision_msgs/BoundingBox2DArray.py b/dimos/msgs/vision_msgs/BoundingBox2DArray.py new file mode 100644 index 0000000000..6568656884 --- /dev/null +++ b/dimos/msgs/vision_msgs/BoundingBox2DArray.py @@ -0,0 +1,19 @@ +# Copyright 2025 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_lcm.vision_msgs.BoundingBox2DArray import BoundingBox2DArray as LCMBoundingBox2DArray + + +class BoundingBox2DArray(LCMBoundingBox2DArray): + msg_name = "vision_msgs.BoundingBox2DArray" diff --git a/dimos/msgs/vision_msgs/BoundingBox3DArray.py b/dimos/msgs/vision_msgs/BoundingBox3DArray.py new file mode 100644 index 0000000000..afa3d793f9 --- /dev/null +++ b/dimos/msgs/vision_msgs/BoundingBox3DArray.py @@ -0,0 +1,19 @@ +# Copyright 2025 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_lcm.vision_msgs.BoundingBox3DArray import BoundingBox3DArray as LCMBoundingBox3DArray + + +class BoundingBox3DArray(LCMBoundingBox3DArray): + msg_name = "vision_msgs.BoundingBox3DArray" diff --git a/dimos/msgs/vision_msgs/Detection2DArray.py b/dimos/msgs/vision_msgs/Detection2DArray.py new file mode 100644 index 0000000000..004f8fd9b3 --- /dev/null +++ b/dimos/msgs/vision_msgs/Detection2DArray.py @@ -0,0 +1,19 @@ +# Copyright 2025 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_lcm.vision_msgs.Detection2DArray import Detection2DArray as LCMDetection2DArray + + +class Detection2DArray(LCMDetection2DArray): + msg_name = "vision_msgs.Detection2DArray" diff --git a/dimos/msgs/vision_msgs/Detection3DArray.py b/dimos/msgs/vision_msgs/Detection3DArray.py new file mode 100644 index 0000000000..21dabb8057 --- /dev/null +++ b/dimos/msgs/vision_msgs/Detection3DArray.py @@ -0,0 +1,19 @@ +# Copyright 2025 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_lcm.vision_msgs.Detection3DArray import Detection3DArray as LCMDetection3DArray + + +class Detection3DArray(LCMDetection3DArray): + msg_name = "vision_msgs.Detection3DArray" diff --git a/dimos/msgs/vision_msgs/__init__.py b/dimos/msgs/vision_msgs/__init__.py new file mode 100644 index 0000000000..af170cbfab --- /dev/null +++ b/dimos/msgs/vision_msgs/__init__.py @@ -0,0 +1,6 @@ +from .BoundingBox2DArray import BoundingBox2DArray +from .BoundingBox3DArray import BoundingBox3DArray +from .Detection2DArray import Detection2DArray +from .Detection3DArray import Detection3DArray + +__all__ = ["BoundingBox2DArray", "BoundingBox3DArray", "Detection2DArray", "Detection3DArray"] diff --git a/dimos/perception/detection2d/__init__.py b/dimos/perception/detection2d/__init__.py index cf61822d7f..b64461b493 100644 --- a/dimos/perception/detection2d/__init__.py +++ b/dimos/perception/detection2d/__init__.py @@ -1,3 +1,3 @@ -from dimos.perception.detection2d.module import Detect2DModule, Detection2DArrayFix +from dimos.perception.detection2d.module import Detect2DModule from dimos.perception.detection2d.utils import * from dimos.perception.detection2d.yolo_2d_det import * diff --git a/dimos/perception/detection2d/module.py b/dimos/perception/detection2d/module.py index 2428891dff..f8bd3a340a 100644 --- a/dimos/perception/detection2d/module.py +++ b/dimos/perception/detection2d/module.py @@ -21,10 +21,10 @@ ) from dimos_lcm.foxglove_msgs.Color import Color from dimos_lcm.foxglove_msgs.Point2 import Point2 +from dimos.msgs.vision_msgs import Detection2DArray from dimos_lcm.vision_msgs import ( BoundingBox2D, Detection2D, - Detection2DArray, ObjectHypothesis, ObjectHypothesisWithPose, Point2D, @@ -43,10 +43,6 @@ from dimos.types.timestamped import to_ros_stamp -class Detection2DArrayFix(Detection2DArray): - msg_name = "vision_msgs.Detection2DArray" - - Bbox = Tuple[float, float, float, float] CenteredBbox = Tuple[float, float, float, float] # yolo and detic have bad output formats @@ -98,9 +94,9 @@ def build_detection2d(image, detection) -> Detection2D: ) -def build_detection2d_array(imageDetections: ImageDetections) -> Detection2DArrayFix: +def build_detection2d_array(imageDetections: ImageDetections) -> Detection2DArray: [image, detections] = imageDetections - return Detection2DArrayFix( + return Detection2DArray( detections_length=len(detections), header=Header(image.ts, "camera_link"), detections=list( @@ -191,7 +187,7 @@ def flatten(xss): class Detect2DModule(Module): image: In[Image] = None - detections: Out[Detection2DArrayFix] = None + detections: Out[Detection2DArray] = None annotations: Out[ImageAnnotations] = None # _initDetector = Detic2DDetector @@ -212,7 +208,7 @@ def start(self): self.annotation_stream().subscribe(self.annotations.publish) @functools.cache - def detection2d_stream(self) -> Observable[Detection2DArrayFix]: + def detection2d_stream(self) -> Observable[Detection2DArray]: return self.image.observable().pipe(ops.map(self.detect), ops.map(build_detection2d_array)) @functools.cache diff --git a/dimos/perception/object_tracker.py b/dimos/perception/object_tracker.py index 8a7968c5c0..7fd5872314 100644 --- a/dimos/perception/object_tracker.py +++ b/dimos/perception/object_tracker.py @@ -21,6 +21,7 @@ from dimos.core import In, Out, Module, rpc from dimos.msgs.std_msgs import Header from dimos.msgs.sensor_msgs import Image, ImageFormat +from dimos.msgs.vision_msgs import Detection2DArray, Detection3DArray from dimos.msgs.geometry_msgs import Vector3, Quaternion, Transform, Pose, PoseStamped from dimos.protocol.tf import TF from dimos.utils.logging_config import setup_logger @@ -28,9 +29,7 @@ # Import LCM messages from dimos_lcm.vision_msgs import ( Detection2D, - Detection2DArray, Detection3D, - Detection3DArray, ObjectHypothesisWithPose, ) from dimos_lcm.sensor_msgs import CameraInfo diff --git a/dimos/robot/unitree_webrtc/modular/ivan_unitree.py b/dimos/robot/unitree_webrtc/modular/ivan_unitree.py index c69f488d50..b96f1e7af5 100644 --- a/dimos/robot/unitree_webrtc/modular/ivan_unitree.py +++ b/dimos/robot/unitree_webrtc/modular/ivan_unitree.py @@ -25,11 +25,12 @@ from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Transform, Twist, Vector3 from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.msgs.sensor_msgs import Image +from dimos.msgs.vision_msgs import Detection2DArray from dimos.navigation.bt_navigator.navigator import BehaviorTreeNavigator, NavigatorState from dimos.navigation.frontier_exploration import WavefrontFrontierExplorer from dimos.navigation.global_planner import AstarPlanner from dimos.navigation.local_planner.holonomic_local_planner import HolonomicLocalPlanner -from dimos.perception.detection2d import Detect2DModule, Detection2DArrayFix +from dimos.perception.detection2d import Detect2DModule from dimos.protocol.pubsub import lcm from dimos.robot.foxglove_bridge import FoxgloveBridge from dimos.robot.unitree_webrtc.modular.connection_module import ConnectionModule @@ -133,7 +134,7 @@ def __init__( detection = dimos.deploy(Detect2DModule) detection.image.connect(connection.video) - detection.detections.transport = LCMTransport("/detections", Detection2DArrayFix) + detection.detections.transport = LCMTransport("/detections", Detection2DArray) detection.annotations.transport = LCMTransport("/annotations", ImageAnnotations) detection.start() diff --git a/dimos/robot/unitree_webrtc/unitree_go2.py b/dimos/robot/unitree_webrtc/unitree_go2.py index 99bb38b589..adc811f277 100644 --- a/dimos/robot/unitree_webrtc/unitree_go2.py +++ b/dimos/robot/unitree_webrtc/unitree_go2.py @@ -28,9 +28,9 @@ from dimos.msgs.geometry_msgs import PoseStamped, Transform, Twist, Vector3, Quaternion from dimos.msgs.nav_msgs import OccupancyGrid, Path from dimos.msgs.sensor_msgs import Image +from dimos.msgs.vision_msgs import Detection2DArray, Detection3DArray from dimos_lcm.std_msgs import String from dimos_lcm.sensor_msgs import CameraInfo -from dimos_lcm.vision_msgs import Detection2DArray, Detection3DArray from dimos.perception.spatial_perception import SpatialMemory from dimos.perception.common.utils import ( extract_pose_from_detection3d,