diff --git a/dimos/msgs/geometry_msgs/Point.py b/dimos/msgs/geometry_msgs/Point.py new file mode 100644 index 0000000000..19e91d1bc8 --- /dev/null +++ b/dimos/msgs/geometry_msgs/Point.py @@ -0,0 +1,33 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from dimos_lcm.geometry_msgs import Point as LCMPoint + + +class Point(LCMPoint): + """DimOS wrapper for geometry_msgs.Point (3D position). + + Inherits x/y/z from LCMPoint. Wire-identical to Vector3 but + semantically represents a position, not a direction/displacement. + """ + + msg_name = "geometry_msgs.Point" + + def __init__(self, x: float = 0.0, y: float = 0.0, z: float = 0.0) -> None: + super().__init__(float(x), float(y), float(z)) + + def __repr__(self) -> str: + return f"Point(x={self.x}, y={self.y}, z={self.z})" diff --git a/dimos/msgs/geometry_msgs/PointStamped.py b/dimos/msgs/geometry_msgs/PointStamped.py new file mode 100644 index 0000000000..6dbe754bbe --- /dev/null +++ b/dimos/msgs/geometry_msgs/PointStamped.py @@ -0,0 +1,100 @@ +# 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 time +from typing import TYPE_CHECKING, BinaryIO + +if TYPE_CHECKING: + from rerun._baseclasses import Archetype + +from dimos_lcm.geometry_msgs import PointStamped as LCMPointStamped + +from dimos.msgs.geometry_msgs.Point import Point +from dimos.types.timestamped import Timestamped + + +class PointStamped(Point, Timestamped): + """A 3D point with timestamp and frame_id. + + Follows the same pattern as PoseStamped(Pose, Timestamped) and + TwistStamped(Twist, Timestamped). Inherits x/y/z from Point + (which inherits from LCMPoint). + """ + + msg_name = "geometry_msgs.PointStamped" + ts: float + frame_id: str + + def __init__( + self, + x: float = 0.0, + y: float = 0.0, + z: float = 0.0, + ts: float = 0.0, + frame_id: str = "", + ) -> None: + self.frame_id = frame_id + self.ts = ts if ts != 0 else time.time() + super().__init__(float(x), float(y), float(z)) + + # -- LCM encode / decode -- + + def lcm_encode(self) -> bytes: + """Encode to LCM binary format.""" + lcm_msg = LCMPointStamped() + lcm_msg.point = self # Works because Point inherits from LCMPoint + [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = self.ros_timestamp() + lcm_msg.header.frame_id = self.frame_id + return lcm_msg.lcm_encode() # type: ignore[no-any-return] + + @classmethod + def lcm_decode(cls, data: bytes | BinaryIO) -> PointStamped: + """Decode from LCM binary format.""" + lcm_msg = LCMPointStamped.lcm_decode(data) + return cls( + x=lcm_msg.point.x, + y=lcm_msg.point.y, + z=lcm_msg.point.z, + ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), + frame_id=lcm_msg.header.frame_id, + ) + + # -- Conversion methods -- + + def to_rerun(self) -> Archetype: + """Convert to rerun Points3D archetype for visualization.""" + import rerun as rr + + return rr.Points3D(positions=[[self.x, self.y, self.z]]) + + def to_pose_stamped(self) -> PoseStamped: + """Convert to PoseStamped with identity quaternion orientation.""" + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + return PoseStamped( + ts=self.ts, + frame_id=self.frame_id, + position=[self.x, self.y, self.z], + orientation=[0.0, 0.0, 0.0, 1.0], + ) + + # -- String representations -- + + def __str__(self) -> str: + return f"PointStamped(point=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], frame_id={self.frame_id!r})" + + def __repr__(self) -> str: + return f"PointStamped(x={self.x}, y={self.y}, z={self.z}, ts={self.ts}, frame_id={self.frame_id!r})" diff --git a/dimos/msgs/geometry_msgs/__init__.py b/dimos/msgs/geometry_msgs/__init__.py index 3c6a742fec..01069d765c 100644 --- a/dimos/msgs/geometry_msgs/__init__.py +++ b/dimos/msgs/geometry_msgs/__init__.py @@ -1,3 +1,5 @@ +from dimos.msgs.geometry_msgs.Point import Point +from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.Pose import Pose, PoseLike, to_pose from dimos.msgs.geometry_msgs.PoseArray import PoseArray from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -14,6 +16,8 @@ from dimos.msgs.geometry_msgs.WrenchStamped import WrenchStamped __all__ = [ + "Point", + "PointStamped", "Pose", "PoseArray", "PoseLike", diff --git a/dimos/msgs/geometry_msgs/test_PointStamped.py b/dimos/msgs/geometry_msgs/test_PointStamped.py new file mode 100644 index 0000000000..965ad9a539 --- /dev/null +++ b/dimos/msgs/geometry_msgs/test_PointStamped.py @@ -0,0 +1,63 @@ +# 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. + +"""Tests for geometry_msgs.PointStamped — msg -> lcm bytes -> msg roundtrip.""" + +import time + +from dimos_lcm.geometry_msgs import Point as LCMPoint + +from dimos.msgs.geometry_msgs.PointStamped import Point, PointStamped + + +def test_point_inherits_lcm() -> None: + """Point wrapper inherits from LCMPoint.""" + assert isinstance(Point(1.0, 2.0, 3.0), LCMPoint) + + +def test_lcm_encode_decode() -> None: + """Test encoding and decoding of PointStamped to/from binary LCM format.""" + source = PointStamped( + x=1.5, + y=-2.5, + z=3.5, + ts=time.time(), + frame_id="/world/grid", + ) + binary_msg = source.lcm_encode() + dest = PointStamped.lcm_decode(binary_msg) + + assert isinstance(dest, PointStamped) + assert dest is not source + assert dest.x == source.x + assert dest.y == source.y + assert dest.z == source.z + assert abs(dest.ts - source.ts) < 1e-6 + assert dest.frame_id == source.frame_id + + +def test_to_pose_stamped() -> None: + """Test conversion to PoseStamped with identity orientation.""" + from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + pt = PointStamped(x=1.0, y=2.0, z=3.0, ts=500.0, frame_id="/map") + pose = pt.to_pose_stamped() + + assert isinstance(pose, PoseStamped) + assert pose.x == 1.0 + assert pose.y == 2.0 + assert pose.z == 3.0 + assert pose.orientation.w == 1.0 + assert pose.ts == 500.0 + assert pose.frame_id == "/map" diff --git a/dimos/robot/unitree/go2/blueprints/smart/_clicked_point_transport.py b/dimos/robot/unitree/go2/blueprints/smart/_clicked_point_transport.py new file mode 100644 index 0000000000..6d42f2fd03 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/_clicked_point_transport.py @@ -0,0 +1,128 @@ +#!/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. + +"""Transport that receives PointStamped clicks from Rerun and delivers PoseStamped. + +Subscribes to an LCM topic carrying PointStamped (published by the Rerun +viewer fork) and converts each message to PoseStamped via +``PointStamped.to_pose_stamped()`` before delivering to stream subscribers. + +No DimOS Module is needed -- the conversion lives entirely inside the +transport layer. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from dimos.core.transport import PubSubTransport +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic as LCMTopic +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from collections.abc import Callable + + from dimos.core.stream import Out, Stream + +logger = setup_logger() + + +class ClickedPointTransport(PubSubTransport[PoseStamped]): + """PubSubTransport that bridges ``/clicked_point`` PointStamped -> PoseStamped. + + Internally subscribes to an LCM topic carrying ``PointStamped`` messages + (e.g. published by the Rerun viewer) and converts each one to + ``PoseStamped`` with identity quaternion via ``PointStamped.to_pose_stamped()``. + + Also supports local ``broadcast()`` so that other in-process producers + (RPC ``set_goal``, agent planners) can still publish ``PoseStamped`` + directly through the same transport. + + Usage in a blueprint:: + + from dimos.msgs.geometry_msgs import PoseStamped + + my_blueprint = autoconnect(...).transports({ + ("goal_request", PoseStamped): ClickedPointTransport(), + }) + """ + + _started: bool = False + + def __init__(self, clicked_point_topic: str = "/clicked_point") -> None: + super().__init__(clicked_point_topic) + self._click_lcm = LCM() + self._click_topic = LCMTopic(clicked_point_topic, PointStamped) + self._subscribers: list[Callable[[PoseStamped], Any]] = [] + + # -- PubSubTransport interface ------------------------------------------- + + def broadcast(self, _: Out[PoseStamped] | None, msg: PoseStamped) -> None: + """Deliver a PoseStamped directly to all local subscribers.""" + for cb in self._subscribers: + cb(msg) + + def subscribe( + self, + callback: Callable[[PoseStamped], Any], + selfstream: Stream[PoseStamped] | None = None, + ) -> Callable[[], None]: + """Subscribe and also start listening for PointStamped clicks on LCM.""" + if not self._started: + self.start() + + self._subscribers.append(callback) + + # Subscribe to the external PointStamped LCM topic; convert on receive. + unsub_lcm = self._click_lcm.subscribe( + self._click_topic, + lambda msg, _topic: self._on_click(msg, callback), + ) + + def unsubscribe() -> None: + if callback in self._subscribers: + self._subscribers.remove(callback) + unsub_lcm() + + return unsubscribe + + # -- Lifecycle ----------------------------------------------------------- + + def start(self) -> None: + if not self._started: + self._click_lcm.start() + self._started = True + + def stop(self) -> None: + if self._started: + self._click_lcm.stop() + self._started = False + + # -- Internal ------------------------------------------------------------ + + @staticmethod + def _on_click( + point_stamped: PointStamped, + callback: Callable[[PoseStamped], Any], + ) -> None: + pose = point_stamped.to_pose_stamped() + logger.info( + "ClickedPointTransport", + point=f"({point_stamped.x:.2f}, {point_stamped.y:.2f}, {point_stamped.z:.2f})", + pose=str(pose), + ) + callback(pose) diff --git a/dimos/robot/unitree/go2/blueprints/smart/test_clicked_point_transport.py b/dimos/robot/unitree/go2/blueprints/smart/test_clicked_point_transport.py new file mode 100644 index 0000000000..04a5c85a38 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/test_clicked_point_transport.py @@ -0,0 +1,140 @@ +#!/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. + +"""Tests for ClickedPointTransport. + +Verifies that PointStamped messages published on /clicked_point LCM +are received as PoseStamped with correct coordinates and identity quaternion. +""" + +import threading +import time + +import pytest + +from dimos.msgs.geometry_msgs.PointStamped import PointStamped +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.protocol.pubsub.impl.lcmpubsub import LCM, Topic as LCMTopic +from dimos.robot.unitree.go2.blueprints.smart._clicked_point_transport import ( + ClickedPointTransport, +) + + +@pytest.fixture() +def transport(): + """Create and start a ClickedPointTransport, stop after test.""" + t = ClickedPointTransport("/test_clicked_point") + yield t + t.stop() + + +@pytest.fixture() +def publisher(): + """An LCM publisher for PointStamped on the test topic.""" + lcm = LCM() + lcm.start() + yield lcm + lcm.stop() + + +class TestClickedPointTransport: + """Unit tests for the converting transport.""" + + def test_receives_point_as_pose(self, transport: ClickedPointTransport, publisher: LCM): + """PointStamped on LCM → subscriber receives PoseStamped.""" + received: list[PoseStamped] = [] + event = threading.Event() + + def on_msg(pose: PoseStamped) -> None: + received.append(pose) + event.set() + + transport.subscribe(on_msg) + + # Publish a PointStamped via LCM. + point = PointStamped(x=1.5, y=2.5, z=0.0, frame_id="map") + topic = LCMTopic("/test_clicked_point", PointStamped) + publisher.publish(topic, point) + + assert event.wait(timeout=5.0), "Timed out waiting for converted message" + assert len(received) == 1 + + pose = received[0] + assert isinstance(pose, PoseStamped) + assert pose.x == pytest.approx(1.5) + assert pose.y == pytest.approx(2.5) + assert pose.z == pytest.approx(0.0) + # Identity quaternion (x, y, z, w) = (0, 0, 0, 1) + assert pose.orientation.x == pytest.approx(0.0) + assert pose.orientation.y == pytest.approx(0.0) + assert pose.orientation.z == pytest.approx(0.0) + assert pose.orientation.w == pytest.approx(1.0) + assert pose.frame_id == "map" + + def test_broadcast_delivers_pose_directly(self, transport: ClickedPointTransport): + """broadcast() delivers PoseStamped to subscribers without LCM.""" + received: list[PoseStamped] = [] + transport.subscribe(lambda pose: received.append(pose)) + + pose = PoseStamped( + position=[3.0, 4.0, 0.0], + orientation=[0.0, 0.0, 0.0, 1.0], + frame_id="map", + ) + transport.broadcast(None, pose) + + assert len(received) == 1 + assert received[0].x == pytest.approx(3.0) + assert received[0].y == pytest.approx(4.0) + + def test_unsubscribe_stops_delivery(self, transport: ClickedPointTransport, publisher: LCM): + """After unsubscribe, no more messages are delivered.""" + received: list[PoseStamped] = [] + unsub = transport.subscribe(lambda pose: received.append(pose)) + + # Unsubscribe immediately. + unsub() + + # Publish — should NOT be received. + point = PointStamped(x=9.0, y=9.0, z=0.0, frame_id="map") + topic = LCMTopic("/test_clicked_point", PointStamped) + publisher.publish(topic, point) + time.sleep(0.5) + + assert len(received) == 0 + + def test_multiple_clicks(self, transport: ClickedPointTransport, publisher: LCM): + """Multiple clicks each produce a PoseStamped.""" + received: list[PoseStamped] = [] + event = threading.Event() + + def on_msg(pose: PoseStamped) -> None: + received.append(pose) + if len(received) >= 3: + event.set() + + transport.subscribe(on_msg) + + topic = LCMTopic("/test_clicked_point", PointStamped) + for i in range(3): + point = PointStamped(x=float(i), y=float(i * 2), z=0.0, frame_id="map") + publisher.publish(topic, point) + time.sleep(0.05) # small gap between publishes + + assert event.wait(timeout=5.0), "Timed out waiting for 3 messages" + assert len(received) == 3 + assert received[0].x == pytest.approx(0.0) + assert received[1].x == pytest.approx(1.0) + assert received[2].x == pytest.approx(2.0) diff --git a/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py new file mode 100644 index 0000000000..3b865e1c94 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/smart/unitree_go2_click_nav.py @@ -0,0 +1,56 @@ +#!/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. + +"""Go2 click-to-navigate blueprint. + +Wires the Rerun viewer's ``/clicked_point`` (PointStamped via LCM) into +the ``ReplanningAStarPlanner``'s ``goal_request`` stream (PoseStamped). + +Click in Rerun → PointStamped → to_pose_stamped() → planner navigates. + +No extra DimOS Module is needed; the type conversion happens inside +``ClickedPointTransport`` at the transport layer. +""" + +from dimos.core.blueprints import autoconnect +from dimos.mapping.costmapper import cost_mapper +from dimos.mapping.voxels import voxel_mapper +from dimos.msgs.geometry_msgs import PoseStamped +from dimos.navigation.replanning_a_star.module import replanning_a_star_planner +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.robot.unitree.go2.blueprints.smart._clicked_point_transport import ( + ClickedPointTransport, +) + +# --------------------------------------------------------------------------- +# Click-to-navigate: basic robot + mapping + planning, goals from Rerun clicks +# --------------------------------------------------------------------------- +unitree_go2_click_nav = ( + autoconnect( + unitree_go2_basic, + voxel_mapper(voxel_size=0.1), + cost_mapper(), + replanning_a_star_planner(), + # No frontier explorer — goals come from Rerun viewer clicks. + ) + .transports( + { + ("goal_request", PoseStamped): ClickedPointTransport(), + } + ) + .global_config(n_workers=5, robot_model="unitree_go2") +) + +__all__ = ["unitree_go2_click_nav"]