diff --git a/dimos/msgs/foxglove_msgs/CompressedVideo.py b/dimos/msgs/foxglove_msgs/CompressedVideo.py deleted file mode 100644 index b13089c303b..00000000000 --- a/dimos/msgs/foxglove_msgs/CompressedVideo.py +++ /dev/null @@ -1,107 +0,0 @@ -# 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. - -"""foxglove_msgs::msg::CompressedVideo — one encoded video packet. - -Just the encoded bytes + codec name. Inter-frame codecs (h264) can't be decoded -one packet at a time — feed the ordered stream through -:class:`~dimos.robot.unitree.go2.dds.video.H264Decoder` to get ``Image`` frames, -or let the rerun bridge log it as a ``VideoStream`` and decode in-viewer. -""" - -from __future__ import annotations - -import time -from typing import TYPE_CHECKING - -from dimos_lcm.foxglove_msgs import CompressedVideo as LCMCompressedVideo -import numpy as np - -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from dimos.visualization.rerun.bridge import RerunData - - -class CompressedVideo(Timestamped): - """One encoded video packet (Annex-B for h264).""" - - msg_name = "foxglove_msgs.CompressedVideo" - - def __init__( - self, - data: np.ndarray | bytes, - format: str = "h264", - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize a CompressedVideo. - - Args: - data: The encoded packet, u8 - format: Codec name, e.g. "h264" - frame_id: Frame ID - ts: Timestamp in seconds - """ - self.data = data if isinstance(data, np.ndarray) else np.frombuffer(data, dtype=np.uint8) - self.format = format - self.frame_id = frame_id - self.ts = ts if ts is not None else time.time() - - def lcm_encode(self) -> bytes: - msg = LCMCompressedVideo() - msg.timestamp.sec = int(self.ts) - msg.timestamp.nanosec = int((self.ts - int(self.ts)) * 1e9) - msg.frame_id = self.frame_id - msg.data = self.data.tobytes() - msg.data_length = len(msg.data) - msg.format = self.format - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> CompressedVideo: - msg = LCMCompressedVideo.lcm_decode(data) - return cls( - data=bytes(msg.data), - format=msg.format, - frame_id=msg.frame_id, - ts=msg.timestamp.sec + msg.timestamp.nanosec * 1e-9, - ) - - def to_rerun(self) -> RerunData: - """Log the encoded packet as a rerun ``VideoStream`` sample (viewer decodes). - - rerun decodes the stream in-viewer, so this stays per-packet and cheap — - no server-side decode, and the .rrd holds the compressed bytes. Iterate - from the start (or a keyframe) so the first sample the viewer sees is one. - """ - import rerun as rr - - codecs = { - "h264": rr.VideoCodec.H264, - "h265": rr.VideoCodec.H265, - "av1": rr.VideoCodec.AV1, - "vp8": rr.VideoCodec.VP8, - "vp9": rr.VideoCodec.VP9, - } - codec = codecs.get(self.format.lower()) - if codec is None: - raise ValueError(f"no rerun VideoCodec for format {self.format!r}") - return rr.VideoStream(codec, sample=self.data.tobytes()) - - def __repr__(self) -> str: - return ( - f"CompressedVideo(format='{self.format}', bytes={self.data.size}, " - f"frame_id='{self.frame_id}', ts={self.ts})" - ) diff --git a/dimos/msgs/geometry_msgs/Point.py b/dimos/msgs/geometry_msgs/Point.py deleted file mode 100644 index 84f260dc8a4..00000000000 --- a/dimos/msgs/geometry_msgs/Point.py +++ /dev/null @@ -1,33 +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 dimos_lcm.geometry_msgs import Point as LCMPoint - - -class Point(LCMPoint): # type: ignore[misc] - """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 deleted file mode 100644 index 8d305d99493..00000000000 --- a/dimos/msgs/geometry_msgs/PointStamped.py +++ /dev/null @@ -1,102 +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 time -from typing import TYPE_CHECKING, BinaryIO - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - -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 | None = None, - frame_id: str = "", - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - 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/Pose.py b/dimos/msgs/geometry_msgs/Pose.py deleted file mode 100644 index 985a677c70f..00000000000 --- a/dimos/msgs/geometry_msgs/Pose.py +++ /dev/null @@ -1,265 +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 typing import Any, TypeAlias, overload - -from dimos_lcm.geometry_msgs import ( - Pose as LCMPose, - Transform as LCMTransform, -) -from plum import dispatch - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable - -# Types that can be converted to/from Pose -PoseConvertable: TypeAlias = ( - tuple[VectorConvertable, QuaternionConvertable] - | LCMPose - | Vector3 - | dict[str, VectorConvertable | QuaternionConvertable] -) - - -def _is_position_orientation_pair(value: Any) -> bool: - """True for a 2-element (position, orientation) pair, false for a 2D position.""" - return ( - isinstance(value, tuple | list) - and len(value) == 2 - and not isinstance(value[0], int | float) - and not isinstance(value[1], int | float) - ) - - -class Pose(LCMPose): # type: ignore[misc] - position: Vector3 - orientation: Quaternion - msg_name = "geometry_msgs.Pose" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__(self, x: int | float, y: int | float, z: int | float) -> None: ... - - @overload - def __init__( - self, - x: int | float, - y: int | float, - z: int | float, - qx: int | float, - qy: int | float, - qz: int | float, - qw: int | float, - ) -> None: ... - - @overload - def __init__( - self, - position: VectorConvertable | Vector3 | None = ..., - orientation: QuaternionConvertable | Quaternion | None = ..., - ) -> None: ... - - @overload - def __init__(self, value: PoseConvertable | Pose, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a pose. - - Supported forms: - Pose() # origin, identity orientation - Pose(x, y, z) - Pose(x, y, z, qx, qy, qz, qw) - Pose(position) # any Vector3-convertable - Pose(position, orientation) - Pose(position=..., orientation=...) # keyword args - Pose((position, orientation)) # pair - Pose({"position": ..., "orientation": ...}) - Pose(other_pose) # copy constructor - Pose(lcm_pose) # from LCM message - """ - position: Any = None - orientation: Any = None - - if len(args) == 3: - position = args - elif len(args) == 7: - position, orientation = args[:3], args[3:] - elif len(args) == 2: - position, orientation = args - elif len(args) == 1: - value = args[0] - # Pose before LCMPose (it is a subclass); dict and pair before the - # generic "it is a position" fallback. - if isinstance(value, Pose): - position, orientation = value.position, value.orientation - elif isinstance(value, LCMPose): - position = (value.position.x, value.position.y, value.position.z) - orientation = ( - value.orientation.x, - value.orientation.y, - value.orientation.z, - value.orientation.w, - ) - elif isinstance(value, dict): - position, orientation = value["position"], value["orientation"] - elif _is_position_orientation_pair(value): - position, orientation = value - else: - position = value - elif args: - raise TypeError(f"Pose takes 1, 2, 3 or 7 positional arguments ({len(args)} given)") - - if kwargs: - position = kwargs.pop("position", position) - orientation = kwargs.pop("orientation", orientation) - if kwargs: - raise TypeError(f"Pose got unexpected keyword arguments {sorted(kwargs)}") - - self.position = Vector3(0.0, 0.0, 0.0) if position is None else Vector3(position) - self.orientation = ( - Quaternion(0.0, 0.0, 0.0, 1.0) if orientation is None else Quaternion(orientation) - ) - - @property - def x(self) -> float: - """X coordinate of position.""" - return self.position.x - - @property - def y(self) -> float: - """Y coordinate of position.""" - return self.position.y - - @property - def z(self) -> float: - """Z coordinate of position.""" - return self.position.z - - @property - def roll(self) -> float: - """Roll angle in radians.""" - return self.orientation.to_euler().roll - - @property - def pitch(self) -> float: - """Pitch angle in radians.""" - return self.orientation.to_euler().pitch - - @property - def yaw(self) -> float: - """Yaw angle in radians.""" - return self.orientation.to_euler().yaw - - def __repr__(self) -> str: - return f"Pose(position={self.position!r}, orientation={self.orientation!r})" - - def __str__(self) -> str: - return ( - f"Pose(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}]), " - f"quaternion=[{self.orientation}])" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two poses are equal.""" - if not isinstance(other, Pose): - return False - return self.position == other.position and self.orientation == other.orientation - - def __matmul__(self, transform: LCMTransform | Transform) -> Pose: - return self + transform - - def __add__(self, other: Pose | PoseConvertable | LCMTransform | Transform) -> Pose: - """Compose two poses or apply a transform (transform composition). - - The operation self + other represents applying transformation 'other' - in the coordinate frame defined by 'self'. This is equivalent to: - - First apply transformation 'self' (from world to self's frame) - - Then apply transformation 'other' (from self's frame to other's frame) - - This matches ROS tf convention where: - T_world_to_other = T_world_to_self * T_self_to_other - - Args: - other: The pose or transform to compose with this one - - Returns: - A new Pose representing the composed transformation - - Example: - robot_pose = Pose(1, 0, 0) # Robot at (1,0,0) facing forward - object_in_robot = Pose(2, 0, 0) # Object 2m in front of robot - object_in_world = robot_pose + object_in_robot # Object at (3,0,0) in world - - # Or with a Transform: - transform = Transform() - transform.translation = Vector3(2, 0, 0) - transform.rotation = Quaternion(0, 0, 0, 1) - new_pose = pose + transform - """ - # Handle Transform objects - if isinstance(other, LCMTransform | Transform): - # Convert Transform to Pose using its translation and rotation - other_position = Vector3(other.translation) - other_orientation = Quaternion(other.rotation) - elif isinstance(other, Pose): - other_position = other.position - other_orientation = other.orientation - else: - # Convert to Pose if it's a convertible type - other_pose = Pose(other) - other_position = other_pose.position - other_orientation = other_pose.orientation - - # Compose orientations: self.orientation * other.orientation - new_orientation = self.orientation * other_orientation - - # Transform other's position by self's orientation, then add to self's position - rotated_position = self.orientation.rotate_vector(other_position) - new_position = self.position + rotated_position - - return Pose(new_position, new_orientation) - - def __sub__(self, other: Pose) -> Pose: - """Compute the delta pose: self - other. - - For position: simple subtraction. - For orientation: delta_quat = self.orientation * inverse(other.orientation) - - Returns: - A new Pose representing the delta transformation - """ - delta_position = self.position - other.position - delta_orientation = self.orientation * other.orientation.inverse() - return Pose(delta_position, delta_orientation) - - -@dispatch -def to_pose(value: Pose) -> Pose: - """Pass through Pose objects.""" - return value - - -@dispatch # type: ignore[no-redef] -def to_pose(value: PoseConvertable) -> Pose: - """Convert a pose-compatible value to a Pose object.""" - return Pose(value) - - -PoseLike: TypeAlias = PoseConvertable | Pose diff --git a/dimos/msgs/geometry_msgs/PoseArray.py b/dimos/msgs/geometry_msgs/PoseArray.py deleted file mode 100644 index e27f56b6bfd..00000000000 --- a/dimos/msgs/geometry_msgs/PoseArray.py +++ /dev/null @@ -1,97 +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. - -"""PoseArray message type for Dimos.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from dimos.msgs.std_msgs.Header import Header - -if TYPE_CHECKING: - from collections.abc import Iterator - - from dimos.msgs.geometry_msgs.Pose import Pose - - -class PoseArray: - """ - An array of poses with a header for reference frame and timestamp. - - This is commonly used for representing multiple candidate positions, - such as grasp poses, particle filter samples, or waypoints. - """ - - msg_name = "geometry_msgs.PoseArray" - - def __init__(self, header: Header | None = None, poses: list[Pose] | None = None) -> None: - """ - Initialize a PoseArray. - - Args: - header: Header with frame_id and timestamp - poses: List of Pose objects - """ - self.header = header if header is not None else Header() - self.poses = poses if poses is not None else [] - - def __repr__(self) -> str: - return f"PoseArray(header={self.header!r}, poses={len(self.poses)} poses)" - - def __str__(self) -> str: - return f"PoseArray(frame_id={self.header.frame_id}, num_poses={len(self.poses)})" - - def __len__(self) -> int: - """Return the number of poses in the array.""" - return len(self.poses) - - def __getitem__(self, index: int) -> Pose: - """Get pose at index.""" - return self.poses[index] - - def __iter__(self) -> Iterator[Pose]: - """Iterate over poses.""" - return iter(self.poses) - - def append(self, pose: Pose) -> None: - """Add a pose to the array.""" - self.poses.append(pose) - - def encode(self) -> bytes: - """ - Encode to bytes for LCM transmission. - - Note: This is a simple implementation. For production use, - consider using proper LCM encoding. - """ - import pickle - - return pickle.dumps({"header": self.header, "poses": self.poses}) - - @classmethod - def decode(cls, data: bytes) -> PoseArray: - """ - Decode from bytes. - - Args: - data: Pickled PoseArray data - - Returns: - Decoded PoseArray - """ - import pickle - - decoded = pickle.loads(data) - return cls(header=decoded["header"], poses=decoded["poses"]) diff --git a/dimos/msgs/geometry_msgs/PoseStamped.py b/dimos/msgs/geometry_msgs/PoseStamped.py deleted file mode 100644 index a78adadd19b..00000000000 --- a/dimos/msgs/geometry_msgs/PoseStamped.py +++ /dev/null @@ -1,155 +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 math -import time -from typing import TYPE_CHECKING, Any, BinaryIO, TypeAlias - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos_lcm.geometry_msgs import PoseStamped as LCMPoseStamped - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion, QuaternionConvertable -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from Pose -PoseConvertable: TypeAlias = ( - tuple[VectorConvertable, QuaternionConvertable] - | LCMPoseStamped - | dict[str, VectorConvertable | QuaternionConvertable] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class PoseStamped(Pose, Timestamped): - msg_name = "geometry_msgs.PoseStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped pose. - - Takes ``(ts, frame_id)`` positionally, plus every Pose keyword. Any other - positional form belongs to Pose, so ``PoseStamped(x, y, z)`` and - ``PoseStamped(x, y, z, qx, qy, qz, qw)`` work as they do on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Pose argument. - if args and len(args) < 3 and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - pose_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - pose_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*pose_args, **kwargs) - - def lcm_encode(self) -> bytes: - lcm_mgs = LCMPoseStamped() - lcm_mgs.pose = self - [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_mgs.header.frame_id = self.frame_id - return lcm_mgs.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped: - lcm_msg = LCMPoseStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z], - orientation=[ - lcm_msg.pose.orientation.x, - lcm_msg.pose.orientation.y, - lcm_msg.pose.orientation.z, - lcm_msg.pose.orientation.w, - ], - ) - - def __str__(self) -> str: - return ( - f"PoseStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{math.degrees(self.roll):.1f}, {math.degrees(self.pitch):.1f}, {math.degrees(self.yaw):.1f}])" - ) - - def to_rerun(self) -> Archetype: - """Convert to rerun Transform3D format. - - Returns a Transform3D that can be logged to Rerun to position - child entities in the transform hierarchy. - """ - import rerun as rr - - return rr.Transform3D( - translation=[self.x, self.y, self.z], - rotation=rr.Quaternion( - xyzw=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ] - ), - ) - - def to_rerun_arrow(self, length: float = 0.5): # type: ignore[no-untyped-def] - """Convert to rerun Arrows3D format for visualization.""" - import rerun as rr - - origin = [[self.x, self.y, self.z]] - forward = self.orientation.rotate_vector(Vector3(length, 0, 0)) - vector = [[forward.x, forward.y, forward.z]] - return rr.Arrows3D(origins=origin, vectors=vector) - - def new_transform_to(self, name: str) -> Transform: - return self.find_transform( - PoseStamped( - frame_id=name, - position=Vector3(0, 0, 0), - orientation=Quaternion(0, 0, 0, 1), # Identity quaternion - ) - ) - - def new_transform_from(self, name: str) -> Transform: - return self.new_transform_to(name).inverse() - - def find_transform(self, other: PoseStamped) -> Transform: - inv_orientation = self.orientation.conjugate() - - pos_diff = other.position - self.position - - local_translation = inv_orientation.rotate_vector(pos_diff) - - relative_rotation = inv_orientation * other.orientation - - return Transform( - child_frame_id=other.frame_id, - frame_id=self.frame_id, - translation=local_translation, - rotation=relative_rotation, - ) diff --git a/dimos/msgs/geometry_msgs/PoseWithCovariance.py b/dimos/msgs/geometry_msgs/PoseWithCovariance.py deleted file mode 100644 index f26a987bee1..00000000000 --- a/dimos/msgs/geometry_msgs/PoseWithCovariance.py +++ /dev/null @@ -1,201 +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 typing import TYPE_CHECKING, Any, TypeAlias - -from dimos_lcm.geometry_msgs import ( - PoseWithCovariance as LCMPoseWithCovariance, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Types that can be converted to/from PoseWithCovariance -PoseWithCovarianceConvertable: TypeAlias = ( - tuple[PoseConvertable, list[float] | np.ndarray] - | LCMPoseWithCovariance - | dict[str, PoseConvertable | list[float] | np.ndarray] -) - -COVARIANCE_SIZE = 36 - - -def _is_value_covariance_pair(value: Any) -> bool: - """True for a 2-element (value, covariance) pair, where covariance is 36 long.""" - if not isinstance(value, tuple | list) or len(value) != 2: - return False - covariance = value[1] - return isinstance(covariance, list | np.ndarray) and np.size(covariance) == COVARIANCE_SIZE - - -class PoseWithCovariance(LCMPoseWithCovariance): # type: ignore[misc] - pose: Pose - covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] - msg_name = "geometry_msgs.PoseWithCovariance" - - def __init__( - self, - pose: Pose | PoseConvertable | PoseWithCovarianceConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize a pose with covariance. - - Supported forms: - PoseWithCovariance() # origin, zero covariance - PoseWithCovariance(pose) - PoseWithCovariance(pose, covariance) - PoseWithCovariance(pose=..., covariance=...) - PoseWithCovariance((pose, covariance)) # pair - PoseWithCovariance({"pose": ..., "covariance": ...}) - PoseWithCovariance(other) # copy constructor - PoseWithCovariance(lcm_pose_with_cov) # from LCM message - """ - source: Any = pose - cov: Any = covariance - - # PoseWithCovariance before LCMPoseWithCovariance (it is a subclass). - if isinstance(source, PoseWithCovariance): - if cov is None: - cov = np.array(source.covariance).copy() - source = Pose(source.pose) # copy constructor: don't alias the source pose - elif isinstance(source, LCMPoseWithCovariance): - if cov is None: - cov = np.array(source.covariance) - source = source.pose - elif isinstance(source, dict) and "pose" in source: - cov = source.get("covariance", cov) - source = source["pose"] - elif _is_value_covariance_pair(source): - source, cov = source - - self.pose = source if isinstance(source, Pose) else Pose(source) - self.covariance = ( - np.zeros(COVARIANCE_SIZE) - if cov is None - else np.array(cov, dtype=float).reshape(COVARIANCE_SIZE) - ) - - def __getattribute__(self, name: str): # type: ignore[no-untyped-def] - """Override to ensure covariance is always returned as numpy array.""" - if name == "covariance": - cov = object.__getattribute__(self, "covariance") - if not isinstance(cov, np.ndarray): - return np.array(cov, dtype=float) - return cov - return super().__getattribute__(name) - - def __setattr__(self, name: str, value) -> None: # type: ignore[no-untyped-def] - """Override to ensure covariance is stored as numpy array.""" - if name == "covariance": - if not isinstance(value, np.ndarray): - value = np.array(value, dtype=float).reshape(36) - super().__setattr__(name, value) - - @property - def x(self) -> float: - """X coordinate of position.""" - return self.pose.x - - @property - def y(self) -> float: - """Y coordinate of position.""" - return self.pose.y - - @property - def z(self) -> float: - """Z coordinate of position.""" - return self.pose.z - - @property - def position(self) -> Vector3: - """Position vector.""" - return self.pose.position - - @property - def orientation(self) -> Quaternion: - """Orientation quaternion.""" - return self.pose.orientation - - @property - def roll(self) -> float: - """Roll angle in radians.""" - return self.pose.roll - - @property - def pitch(self) -> float: - """Pitch angle in radians.""" - return self.pose.pitch - - @property - def yaw(self) -> float: - """Yaw angle in radians.""" - return self.pose.yaw - - @property - def covariance_matrix(self) -> np.ndarray: - """Get covariance as 6x6 matrix.""" - return self.covariance.reshape(6, 6) - - @covariance_matrix.setter - def covariance_matrix(self, value: np.ndarray) -> None: - """Set covariance from 6x6 matrix.""" - self.covariance = np.array(value).reshape(36) - - def __repr__(self) -> str: - return f"PoseWithCovariance(pose={self.pose!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" - - def __str__(self) -> str: - return ( - f"PoseWithCovariance(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two PoseWithCovariance are equal.""" - if not isinstance(other, PoseWithCovariance): - return False - return self.pose == other.pose and np.allclose(self.covariance, other.covariance) - - def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" - lcm_msg = LCMPoseWithCovariance() - lcm_msg.pose = self.pose - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.covariance = self.covariance.tolist() - else: - lcm_msg.covariance = list(self.covariance) - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> PoseWithCovariance: - """Decode from LCM binary format.""" - lcm_msg = LCMPoseWithCovariance.lcm_decode(data) - pose = Pose( - position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z], - orientation=[ - lcm_msg.pose.orientation.x, - lcm_msg.pose.orientation.y, - lcm_msg.pose.orientation.z, - lcm_msg.pose.orientation.w, - ], - ) - return cls(pose, lcm_msg.covariance) diff --git a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py deleted file mode 100644 index 17f3f2b6d41..00000000000 --- a/dimos/msgs/geometry_msgs/PoseWithCovarianceStamped.py +++ /dev/null @@ -1,98 +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 time -from typing import TypeAlias - -from dimos_lcm.geometry_msgs import ( - PoseWithCovarianceStamped as LCMPoseWithCovarianceStamped, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose, PoseConvertable -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from PoseWithCovarianceStamped -PoseWithCovarianceStampedConvertable: TypeAlias = ( - tuple[PoseConvertable, list[float] | np.ndarray] - | LCMPoseWithCovarianceStamped - | dict[str, PoseConvertable | list[float] | np.ndarray | float | str] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class PoseWithCovarianceStamped(PoseWithCovariance, Timestamped): - msg_name = "geometry_msgs.PoseWithCovarianceStamped" - ts: float - frame_id: str - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - pose: Pose | PoseConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize with timestamp, frame_id, pose and covariance.""" - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(pose, covariance) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMPoseWithCovarianceStamped() - lcm_msg.pose.pose = self.pose - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.pose.covariance = self.covariance.tolist() - else: - lcm_msg.pose.covariance = list(self.covariance) - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - 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) -> PoseWithCovarianceStamped: - lcm_msg = LCMPoseWithCovarianceStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - pose=Pose( - position=[ - lcm_msg.pose.pose.position.x, - lcm_msg.pose.pose.position.y, - lcm_msg.pose.pose.position.z, - ], - orientation=[ - lcm_msg.pose.pose.orientation.x, - lcm_msg.pose.pose.orientation.y, - lcm_msg.pose.pose.orientation.z, - lcm_msg.pose.pose.orientation.w, - ], - ), - covariance=lcm_msg.pose.covariance, - ) - - def __str__(self) -> str: - return ( - f"PoseWithCovarianceStamped(pos=[{self.x:.3f}, {self.y:.3f}, {self.z:.3f}], " - f"euler=[{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) diff --git a/dimos/msgs/geometry_msgs/Quaternion.py b/dimos/msgs/geometry_msgs/Quaternion.py deleted file mode 100644 index e782bc71142..00000000000 --- a/dimos/msgs/geometry_msgs/Quaternion.py +++ /dev/null @@ -1,325 +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 Sequence -from io import BytesIO -import math -import struct -from typing import TYPE_CHECKING, Any, BinaryIO, TypeAlias, overload - -if TYPE_CHECKING: - import rerun as rr - -from dimos_lcm.geometry_msgs import Quaternion as LCMQuaternion -import numpy as np - -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -# Types that can be converted to/from Quaternion -QuaternionConvertable: TypeAlias = Sequence[int | float] | LCMQuaternion | np.ndarray - - -def _four_components(value: Any) -> tuple[float, float, float, float]: - """Unpack a length-4 sequence or array into (x, y, z, w).""" - if not isinstance(value, np.ndarray | Sequence): - raise TypeError(f"Cannot initialize Quaternion from {type(value)}") - size = value.size if isinstance(value, np.ndarray) else len(value) - if size != 4: - raise ValueError("Quaternion requires exactly 4 components [x, y, z, w]") - return (float(value[0]), float(value[1]), float(value[2]), float(value[3])) - - -class Quaternion(LCMQuaternion): # type: ignore[misc] - x: float = 0.0 - y: float = 0.0 - z: float = 0.0 - w: float = 1.0 - msg_name = "geometry_msgs.Quaternion" - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO): # type: ignore[no-untyped-def] - if not hasattr(data, "read"): - data = BytesIO(data) - if data.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._lcm_decode_one(data) # type: ignore[no-untyped-call] - - @classmethod - def _lcm_decode_one(cls, buf): # type: ignore[no-untyped-def] - return cls(struct.unpack(">dddd", buf.read(32))) - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - x: int | float = ..., - y: int | float = ..., - z: int | float = ..., - w: int | float = ..., - ) -> None: ... - - @overload - def __init__(self, value: QuaternionConvertable | Quaternion, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a quaternion. - - Supported forms: - Quaternion() # identity (0, 0, 0, 1) - Quaternion(x, y, z, w) - Quaternion(x=1, y=2, z=3, w=4) # keyword args - Quaternion([x, y, z, w]) # sequence - Quaternion(np.array([x, y, z, w])) # numpy array - Quaternion(other_quaternion) # copy constructor - Quaternion(lcm_quaternion) # from LCM message - """ - if len(args) == 4: - self.x = float(args[0]) - self.y = float(args[1]) - self.z = float(args[2]) - self.w = float(args[3]) - elif len(args) == 1: - value = args[0] - # Quaternion before LCMQuaternion (it is a subclass) and before the - # generic sequence branch (a Quaternion is indexable). - if isinstance(value, Quaternion): - self.x, self.y, self.z, self.w = value.x, value.y, value.z, value.w - elif isinstance(value, LCMQuaternion): - self.x, self.y, self.z, self.w = value.x, value.y, value.z, value.w - else: - self.x, self.y, self.z, self.w = _four_components(value) - elif args: - raise TypeError( - f"Quaternion takes 1 sequence or 4 components ({len(args)} positional given)" - ) - elif kwargs: - self.x = float(kwargs.pop("x", 0.0)) - self.y = float(kwargs.pop("y", 0.0)) - self.z = float(kwargs.pop("z", 0.0)) - self.w = float(kwargs.pop("w", 1.0)) - # else: no arguments — the class defaults already spell the identity. - - if kwargs: - raise TypeError(f"Quaternion got unexpected keyword arguments {sorted(kwargs)}") - - def to_tuple(self) -> tuple[float, float, float, float]: - """Tuple representation of the quaternion (x, y, z, w).""" - return (self.x, self.y, self.z, self.w) - - def to_list(self) -> list[float]: - """List representation of the quaternion (x, y, z, w).""" - return [self.x, self.y, self.z, self.w] - - def to_numpy(self) -> np.ndarray: - """Numpy array representation of the quaternion (x, y, z, w).""" - return np.array([self.x, self.y, self.z, self.w]) - - @property - def euler(self) -> Vector3: - return self.to_euler() - - @property - def radians(self) -> Vector3: - return self.to_euler() - - def to_radians(self) -> Vector3: - """Radians representation of the quaternion (x, y, z, w).""" - return self.to_euler() - - def to_rerun(self) -> rr.Quaternion: - import rerun as rr - - return rr.Quaternion(xyzw=[self.x, self.y, self.z, self.w]) - - @classmethod - def from_euler(cls, vector: Vector3) -> Quaternion: - """Convert Euler angles (roll, pitch, yaw) in radians to quaternion. - - Args: - vector: Vector3 containing (roll, pitch, yaw) in radians - - Returns: - Quaternion representation - """ - - # Calculate quaternion components - cy = np.cos(vector.yaw * 0.5) - sy = np.sin(vector.yaw * 0.5) - cp = np.cos(vector.pitch * 0.5) - sp = np.sin(vector.pitch * 0.5) - cr = np.cos(vector.roll * 0.5) - sr = np.sin(vector.roll * 0.5) - - w = cr * cp * cy + sr * sp * sy - x = sr * cp * cy - cr * sp * sy - y = cr * sp * cy + sr * cp * sy - z = cr * cp * sy - sr * sp * cy - - return cls(x, y, z, w) - - @classmethod - def from_rotation_matrix(cls, matrix: np.ndarray) -> Quaternion: - """Convert a 3x3 rotation matrix to quaternion. - - Args: - matrix: 3x3 rotation matrix (numpy array) - - Returns: - Quaternion representation - """ - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - rotation = Rotation.from_matrix(matrix) - quat = rotation.as_quat() # Returns [x, y, z, w] - return cls(quat[0], quat[1], quat[2], quat[3]) - - def to_rotation_matrix(self) -> np.ndarray: - """Convert quaternion to a 3x3 rotation matrix.""" - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - return np.asarray(Rotation.from_quat([self.x, self.y, self.z, self.w]).as_matrix()) - - def to_euler(self) -> Vector3: - """Convert quaternion to Euler angles (roll, pitch, yaw) in radians. - - Returns: - Vector3: Euler angles as (roll, pitch, yaw) in radians - """ - # Use scipy for accurate quaternion to euler conversion - from scipy.spatial.transform import ( - Rotation, # ~330ms: deferred to avoid startup cost - ) - - quat = [self.x, self.y, self.z, self.w] - rotation = Rotation.from_quat(quat) - euler_angles = rotation.as_euler("xyz") # roll, pitch, yaw - - return Vector3(euler_angles[0], euler_angles[1], euler_angles[2]) - - def __getitem__(self, idx: int) -> float: - """Allow indexing into quaternion components: 0=x, 1=y, 2=z, 3=w.""" - if idx == 0: - return self.x - elif idx == 1: - return self.y - elif idx == 2: - return self.z - elif idx == 3: - return self.w - else: - raise IndexError(f"Quaternion index {idx} out of range [0-3]") - - def __repr__(self) -> str: - return f"Quaternion({self.x:.6f}, {self.y:.6f}, {self.z:.6f}, {self.w:.6f})" - - def __str__(self) -> str: - return self.__repr__() - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - if not isinstance(other, Quaternion): - return False - return self.x == other.x and self.y == other.y and self.z == other.z and self.w == other.w - - def is_zero(self) -> bool: - """All components are zero — i.e. an uninitialized placeholder, not a valid rotation.""" - return self.x == 0.0 and self.y == 0.0 and self.z == 0.0 and self.w == 0.0 - - def __mul__(self, other: Quaternion) -> Quaternion: - """Multiply two quaternions (Hamilton product). - - The result represents the composition of rotations: - q1 * q2 represents rotating by q2 first, then by q1. - """ - if not isinstance(other, Quaternion): - raise TypeError(f"Cannot multiply Quaternion with {type(other)}") - - # Hamilton product formula - w = self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z - x = self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y - y = self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x - z = self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w - - return Quaternion(x, y, z, w) - - def dot(self, other: Quaternion) -> float: - return float(self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w) - - def angle_to(self, other: Quaternion) -> float: - """Smallest rotation angle (radians) between two unit quaternions. - - ``abs(self.dot(other))`` collapses the double-cover sign ambiguity; - the ``min(1.0, ...)`` clamps against numerical drift past 1. - """ - return 2.0 * math.acos(min(1.0, abs(self.dot(other)))) - - def conjugate(self) -> Quaternion: - """Return the conjugate of the quaternion. - - For unit quaternions, the conjugate represents the inverse rotation. - """ - return Quaternion(-self.x, -self.y, -self.z, self.w) - - def inverse(self) -> Quaternion: - """Return the inverse of the quaternion. - - For unit quaternions, this is equivalent to the conjugate. - For non-unit quaternions, this is conjugate / norm^2. - """ - norm_sq = self.x**2 + self.y**2 + self.z**2 + self.w**2 - if norm_sq == 0: - raise ZeroDivisionError("Cannot invert zero quaternion") - - # For unit quaternions (norm_sq ≈ 1), this simplifies to conjugate - if np.isclose(norm_sq, 1.0): - return self.conjugate() - - # For non-unit quaternions - conj = self.conjugate() - return Quaternion(conj.x / norm_sq, conj.y / norm_sq, conj.z / norm_sq, conj.w / norm_sq) - - def normalize(self) -> Quaternion: - """Return a normalized (unit) quaternion.""" - norm = np.sqrt(self.x**2 + self.y**2 + self.z**2 + self.w**2) - if norm == 0: - raise ZeroDivisionError("Cannot normalize zero quaternion") - return Quaternion(self.x / norm, self.y / norm, self.z / norm, self.w / norm) - - def rotate_vector(self, vector: Vector3) -> Vector3: - """Rotate a 3D vector by this quaternion. - - Args: - vector: The vector to rotate - - Returns: - The rotated vector - """ - # For unit quaternions, conjugate equals inverse, so we use conjugate for efficiency - # The rotation formula is: q * v * q^* where q^* is the conjugate - - # Convert vector to pure quaternion (w=0) - v_quat = Quaternion(vector.x, vector.y, vector.z, 0) - - # Apply rotation: q * v * q^* (conjugate for unit quaternions) - rotated = self * v_quat * self.conjugate() - - # Extract vector components - return Vector3(rotated.x, rotated.y, rotated.z) diff --git a/dimos/msgs/geometry_msgs/Transform.py b/dimos/msgs/geometry_msgs/Transform.py deleted file mode 100644 index 6499905da1a..00000000000 --- a/dimos/msgs/geometry_msgs/Transform.py +++ /dev/null @@ -1,329 +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 time -from typing import TYPE_CHECKING, Any, BinaryIO - -if TYPE_CHECKING: - import numpy as np - import rerun as rr - - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - -from dimos_lcm.geometry_msgs import ( - Transform as LCMTransform, - TransformStamped as LCMTransformStamped, -) - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Header import Header -from dimos.types.timestamped import Timestamped - - -class Transform(Timestamped): - translation: Vector3 - rotation: Quaternion - ts: float - frame_id: str - child_frame_id: str - msg_name = "tf2_msgs.TFMessage" - - def __init__( # type: ignore[no-untyped-def] - self, - translation: Vector3 | None = None, - rotation: Quaternion | None = None, - frame_id: str = "world", - child_frame_id: str = "unset", - ts: float | None = None, - **kwargs, - ) -> None: - self.frame_id = frame_id - self.child_frame_id = child_frame_id - self.ts = time.time() if ts is None else ts - self.translation = translation if translation is not None else Vector3() - self.rotation = rotation if rotation is not None else Quaternion() - - def now(self) -> Transform: - """Return a copy of this Transform with the current timestamp.""" - return Transform( - translation=self.translation, - rotation=self.rotation, - frame_id=self.frame_id, - child_frame_id=self.child_frame_id, - ts=time.time(), - ) - - def __repr__(self) -> str: - return f"Transform(translation={self.translation!r}, rotation={self.rotation!r})" - - def __str__(self) -> str: - return f"{self.frame_id} -> {self.child_frame_id}\n Translation: {self.translation}\n Rotation: {self.rotation}" - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two transforms are equal.""" - if not isinstance(other, Transform): - return False - return self.translation == other.translation and self.rotation == other.rotation - - @classmethod - def identity(cls) -> Transform: - """Create an identity transform.""" - return cls() - - def lcm_transform(self) -> LCMTransformStamped: - return LCMTransformStamped( - child_frame_id=self.child_frame_id, - header=Header(self.ts, self.frame_id), - transform=LCMTransform( - translation=self.translation, - rotation=self.rotation, - ), - ) - - def apply(self, other: Transform) -> Transform: - return self.__add__(other) - - def __add__(self, other: Transform) -> Transform: - """Compose two transforms (transform composition). - - The operation self + other represents applying transformation 'other' - in the coordinate frame defined by 'self'. This is equivalent to: - - First apply transformation 'self' (from frame A to frame B) - - Then apply transformation 'other' (from frame B to frame C) - - Args: - other: The transform to compose with this one - - Returns: - A new Transform representing the composed transformation - - Example: - t1 = Transform(Vector3(1, 0, 0), Quaternion(0, 0, 0, 1)) - t2 = Transform(Vector3(2, 0, 0), Quaternion(0, 0, 0, 1)) - t3 = t1 + t2 # Combined transform: translation (3, 0, 0) - """ - if not isinstance(other, Transform): - raise TypeError(f"Cannot add Transform and {type(other).__name__}") - - # Compose orientations: self.rotation * other.rotation - new_rotation = self.rotation * other.rotation - - # Transform other's translation by self's rotation, then add to self's translation - rotated_translation = self.rotation.rotate_vector(other.translation) - new_translation = self.translation + rotated_translation - - return Transform( - translation=new_translation, - rotation=new_rotation, - frame_id=self.frame_id, - child_frame_id=other.child_frame_id, - ts=self.ts, - ) - - def inverse(self) -> Transform: - """Compute the inverse transform. - - The inverse transform reverses the direction of the transformation. - If this transform goes from frame A to frame B, the inverse goes from B to A. - - Returns: - A new Transform representing the inverse transformation - """ - # Inverse rotation - inv_rotation = self.rotation.inverse() - - # Inverse translation: -R^(-1) * t - inv_translation = inv_rotation.rotate_vector(self.translation) - inv_translation = Vector3(-inv_translation.x, -inv_translation.y, -inv_translation.z) - - return Transform( - translation=inv_translation, - rotation=inv_rotation, - frame_id=self.child_frame_id, # Swap frame references - child_frame_id=self.frame_id, - ts=self.ts, - ) - - def __neg__(self) -> Transform: - """Unary minus operator returns the inverse transform.""" - return self.inverse() - - @classmethod - def from_pose(cls, frame_id: str, pose: Pose | PoseStamped) -> Transform: # type: ignore[name-defined] - """Create a Transform from a Pose or PoseStamped. - - Args: - pose: A Pose or PoseStamped object to convert - - Returns: - A Transform with the same translation and rotation as the pose - """ - # Import locally to avoid circular imports - from dimos.msgs.geometry_msgs.Pose import Pose - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - - # Handle both Pose and PoseStamped - if isinstance(pose, PoseStamped): - return cls( - translation=pose.position, - rotation=pose.orientation, - frame_id=pose.frame_id, - child_frame_id=frame_id, - ts=pose.ts, - ) - elif isinstance(pose, Pose): - return cls( - translation=pose.position, - rotation=pose.orientation, - child_frame_id=frame_id, - ) - else: - raise TypeError(f"Expected Pose or PoseStamped, got {type(pose).__name__}") - - def to_pose(self, **kwargs: Any) -> PoseStamped: - """Create a Transform from a Pose or PoseStamped. - - Args: - pose: A Pose or PoseStamped object to convert - - Returns: - A Transform with the same translation and rotation as the pose - """ - # Import locally to avoid circular imports - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped as _PoseStamped - - # Handle both Pose and PoseStamped - fields: dict[str, Any] = { - "position": self.translation, - "orientation": self.rotation, - "frame_id": self.frame_id, - } - result: PoseStamped = _PoseStamped(**fields, **kwargs) - return result - - @classmethod - def from_matrix( - cls, - matrix: np.ndarray, - *, - ts: float | None = None, - frame_id: str = "world", - child_frame_id: str = "unset", - ) -> Transform: - """Build a Transform from a 4x4 homogeneous transformation matrix.""" - return cls( - translation=Vector3(matrix[:3, 3]), - rotation=Quaternion.from_rotation_matrix(matrix[:3, :3]), - frame_id=frame_id, - child_frame_id=child_frame_id, - ts=ts, - ) - - def to_matrix(self) -> np.ndarray: # type: ignore[name-defined] - """Convert Transform to a 4x4 transformation matrix. - - Returns a homogeneous transformation matrix that represents both - the rotation and translation of this transform. - - Returns: - np.ndarray: A 4x4 homogeneous transformation matrix - """ - import numpy as np - - # Extract quaternion components - x, y, z, w = self.rotation.x, self.rotation.y, self.rotation.z, self.rotation.w - - # Build rotation matrix from quaternion using standard formula - # This avoids numerical issues compared to converting to axis-angle first - rotation_matrix = np.array( - [ - [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], - [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], - [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], - ] - ) - - # Build 4x4 homogeneous transformation matrix - matrix = np.eye(4) - matrix[:3, :3] = rotation_matrix - matrix[:3, 3] = [self.translation.x, self.translation.y, self.translation.z] - - return matrix - - def lcm_encode(self) -> bytes: - # we get a circular import otherwise - from dimos.msgs.tf2_msgs.TFMessage import TFMessage - - return TFMessage(self).lcm_encode() - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> Transform: - """Decode from LCM TFMessage bytes.""" - from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage - - lcm_msg = LCMTFMessage.lcm_decode(data) - - if not lcm_msg.transforms: - raise ValueError("No transforms found in LCM message") - - # Get the first transform from the message - lcm_transform_stamped = lcm_msg.transforms[0] - - # Extract timestamp from header - ts = lcm_transform_stamped.header.stamp.sec + ( - lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 - ) - - # Create and return Transform instance - return cls( - translation=Vector3( - lcm_transform_stamped.transform.translation.x, - lcm_transform_stamped.transform.translation.y, - lcm_transform_stamped.transform.translation.z, - ), - rotation=Quaternion( - lcm_transform_stamped.transform.rotation.x, - lcm_transform_stamped.transform.rotation.y, - lcm_transform_stamped.transform.rotation.z, - lcm_transform_stamped.transform.rotation.w, - ), - frame_id=lcm_transform_stamped.header.frame_id, - child_frame_id=lcm_transform_stamped.child_frame_id, - ts=ts, - ) - - def to_rerun(self, frameless: bool = False) -> rr.Transform3D: - """Convert to a rerun Transform3D. - - Args: - frameless: omit ``parent_frame``/``child_frame``. By default the - transform carries its frame IDs (resolved via rerun's tf-graph); - set this when positioning by entity-path hierarchy instead, where - the named frames would not compose. - """ - import rerun as rr - - translation = [self.translation.x, self.translation.y, self.translation.z] - rotation = self.rotation.to_rerun() - if frameless: - return rr.Transform3D(translation=translation, rotation=rotation) - return rr.Transform3D( - translation=translation, - rotation=rotation, - parent_frame="tf#/" + self.frame_id, - child_frame="tf#/" + self.child_frame_id, - ) diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py deleted file mode 100644 index d08f61211f0..00000000000 --- a/dimos/msgs/geometry_msgs/Twist.py +++ /dev/null @@ -1,132 +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 typing import Any, overload - -from dimos_lcm.geometry_msgs import Twist as LCMTwist - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike - - -class Twist(LCMTwist): # type: ignore[misc] - linear: Vector3 - angular: Vector3 - msg_name = "geometry_msgs.Twist" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - linear: VectorLike | None = ..., - angular: VectorLike | Quaternion | None = ..., - ) -> None: ... - - @overload - def __init__(self, twist: Twist | LCMTwist, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a twist. - - Supported forms: - Twist() # zero twist - Twist(linear, angular) - Twist(linear=..., angular=...) # keyword args, either optional - Twist(linear, quaternion) # angular converted to euler - Twist(other_twist) # copy constructor - Twist(lcm_twist) # from LCM message - """ - linear: Any = None - angular: Any = None - - if len(args) == 2: - linear, angular = args - elif len(args) == 1: - value = args[0] - # Twist before LCMTwist (it is a subclass); anything else is a - # linear velocity, same as the `linear=` keyword. - if isinstance(value, Twist | LCMTwist): - linear, angular = value.linear, value.angular - else: - linear = value - elif args: - raise TypeError(f"Twist takes 1 or 2 positional arguments ({len(args)} given)") - - if kwargs: - linear = kwargs.pop("linear", linear) - angular = kwargs.pop("angular", angular) - if kwargs: - raise TypeError(f"Twist got unexpected keyword arguments {sorted(kwargs)}") - - self.linear = Vector3() if linear is None else Vector3(linear) - if angular is None: - self.angular = Vector3() - elif isinstance(angular, Quaternion): - self.angular = angular.to_euler() - else: - self.angular = Vector3(angular) - - def __repr__(self) -> str: - return f"Twist(linear={self.linear!r}, angular={self.angular!r})" - - def __str__(self) -> str: - return f"Twist:\n Linear: {self.linear}\n Angular: {self.angular}" - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two twists are equal.""" - if not isinstance(other, Twist): - return False - return self.linear == other.linear and self.angular == other.angular - - @classmethod - def zero(cls) -> Twist: - """Create a zero twist (no motion).""" - return cls() - - def is_zero(self) -> bool: - """Check if this is a zero twist (no linear or angular velocity).""" - return self.linear.is_zero() and self.angular.is_zero() - - def __sub__(self, other: Twist) -> Twist: - """Component-wise subtraction: self - other.""" - if not isinstance(other, Twist): - return NotImplemented - return Twist( - linear=self.linear - other.linear, - angular=self.angular - other.angular, - ) - - def __add__(self, other: Twist) -> Twist: - """Component-wise addition: self + other.""" - if not isinstance(other, Twist): - return NotImplemented - return Twist( - linear=self.linear + other.linear, - angular=self.angular + other.angular, - ) - - def __bool__(self) -> bool: - """Boolean conversion for Twist. - - A Twist is considered False if it's a zero twist (no motion), - and True otherwise. - - Returns: - False if twist is zero, True otherwise - """ - return not self.is_zero() diff --git a/dimos/msgs/geometry_msgs/TwistStamped.py b/dimos/msgs/geometry_msgs/TwistStamped.py deleted file mode 100644 index 4377687e564..00000000000 --- a/dimos/msgs/geometry_msgs/TwistStamped.py +++ /dev/null @@ -1,85 +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 time -from typing import Any, BinaryIO, TypeAlias - -from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from TwistStamped -TwistConvertable: TypeAlias = ( - tuple[VectorConvertable, VectorConvertable] | LCMTwistStamped | dict[str, VectorConvertable] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class TwistStamped(Twist, Timestamped): - msg_name = "geometry_msgs.TwistStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped twist. - - Takes ``(ts, frame_id)`` positionally, plus every Twist keyword. Any other - positional form belongs to Twist, so ``TwistStamped(linear, angular)`` - works as it does on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Twist argument. - if args and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - twist_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - twist_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*twist_args, **kwargs) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMTwistStamped() - lcm_msg.twist = self - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - 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) -> TwistStamped: - lcm_msg = LCMTwistStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], - angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], - ) - - def __str__(self) -> str: - return ( - f"TwistStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}])" - ) diff --git a/dimos/msgs/geometry_msgs/TwistWithCovariance.py b/dimos/msgs/geometry_msgs/TwistWithCovariance.py deleted file mode 100644 index 3786a013f54..00000000000 --- a/dimos/msgs/geometry_msgs/TwistWithCovariance.py +++ /dev/null @@ -1,181 +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 typing import Any, TypeAlias - -from dimos_lcm.geometry_msgs import ( - TwistWithCovariance as LCMTwistWithCovariance, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorConvertable - -# Types that can be converted to/from TwistWithCovariance -TwistWithCovarianceConvertable: TypeAlias = ( - tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] - | LCMTwistWithCovariance - | dict[str, Twist | tuple[VectorConvertable, VectorConvertable] | list[float] | np.ndarray] -) - -COVARIANCE_SIZE = 36 - - -def _is_twist_covariance_pair(value: Any) -> bool: - """True for a 2-element (twist, covariance) pair, where covariance is 36 long.""" - if not isinstance(value, tuple | list) or len(value) != 2: - return False - covariance = value[1] - return isinstance(covariance, list | np.ndarray) and np.size(covariance) == COVARIANCE_SIZE - - -def _to_twist(value: Any) -> Twist: - """A Twist, or a (linear, angular) pair to build one from.""" - if isinstance(value, Twist): - return value - if value is None: - return Twist() - return Twist(value[0], value[1]) - - -class TwistWithCovariance(LCMTwistWithCovariance): # type: ignore[misc] - twist: Twist - covariance: np.ndarray[tuple[int], np.dtype[np.floating[Any]]] - msg_name = "geometry_msgs.TwistWithCovariance" - - def __init__( - self, - twist: Twist | TwistWithCovarianceConvertable | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize a twist with covariance. - - Supported forms: - TwistWithCovariance() # zero twist and covariance - TwistWithCovariance(twist) - TwistWithCovariance(twist, covariance) - TwistWithCovariance(twist=..., covariance=...) - TwistWithCovariance((linear, angular)) - TwistWithCovariance((twist, covariance)) # pair - TwistWithCovariance({"twist": ..., "covariance": ...}) - TwistWithCovariance(other) # copy constructor - TwistWithCovariance(lcm_twist_with_cov) # from LCM message - """ - source: Any = twist - cov: Any = covariance - - # TwistWithCovariance before LCMTwistWithCovariance (it is a subclass). - if isinstance(source, TwistWithCovariance): - if cov is None: - cov = np.array(source.covariance).copy() - source = Twist(source.twist) # copy constructor: don't alias the source twist - elif isinstance(source, LCMTwistWithCovariance): - if cov is None: - cov = np.array(source.covariance) - source = Twist(source.twist) - elif isinstance(source, dict) and "twist" in source: - cov = source.get("covariance", cov) - source = source["twist"] - elif _is_twist_covariance_pair(source): - source, cov = source - - self.twist = _to_twist(source) - self.covariance = ( - np.zeros(COVARIANCE_SIZE) - if cov is None - else np.array(cov, dtype=float).reshape(COVARIANCE_SIZE) - ) - - def __getattribute__(self, name: str): # type: ignore[no-untyped-def] - """Override to ensure covariance is always returned as numpy array.""" - if name == "covariance": - cov = object.__getattribute__(self, "covariance") - if not isinstance(cov, np.ndarray): - return np.array(cov, dtype=float) - return cov - return super().__getattribute__(name) - - def __setattr__(self, name: str, value) -> None: # type: ignore[no-untyped-def] - """Override to ensure covariance is stored as numpy array.""" - if name == "covariance": - if not isinstance(value, np.ndarray): - value = np.array(value, dtype=float).reshape(36) - super().__setattr__(name, value) - - @property - def linear(self) -> Vector3: - """Linear velocity vector.""" - return self.twist.linear - - @property - def angular(self) -> Vector3: - """Angular velocity vector.""" - return self.twist.angular - - @property - def covariance_matrix(self) -> np.ndarray: - """Get covariance as 6x6 matrix.""" - return self.covariance.reshape(6, 6) - - @covariance_matrix.setter - def covariance_matrix(self, value: np.ndarray) -> None: - """Set covariance from 6x6 matrix.""" - self.covariance = np.array(value).reshape(36) - - def __repr__(self) -> str: - return f"TwistWithCovariance(twist={self.twist!r}, covariance=<{self.covariance.shape[0] if isinstance(self.covariance, np.ndarray) else len(self.covariance)} elements>)" - - def __str__(self) -> str: - return ( - f"TwistWithCovariance(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two TwistWithCovariance are equal.""" - if not isinstance(other, TwistWithCovariance): - return False - return self.twist == other.twist and np.allclose(self.covariance, other.covariance) - - def is_zero(self) -> bool: - """Check if this is a zero twist (no linear or angular velocity).""" - return self.twist.is_zero() - - def __bool__(self) -> bool: - """Boolean conversion - False if zero twist, True otherwise.""" - return not self.is_zero() - - def lcm_encode(self) -> bytes: - """Encode to LCM binary format.""" - lcm_msg = LCMTwistWithCovariance() - lcm_msg.twist = self.twist - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.covariance = self.covariance.tolist() - else: - lcm_msg.covariance = list(self.covariance) - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> TwistWithCovariance: - """Decode from LCM binary format.""" - lcm_msg = LCMTwistWithCovariance.lcm_decode(data) - twist = Twist( - linear=[lcm_msg.twist.linear.x, lcm_msg.twist.linear.y, lcm_msg.twist.linear.z], - angular=[lcm_msg.twist.angular.x, lcm_msg.twist.angular.y, lcm_msg.twist.angular.z], - ) - return cls(twist, lcm_msg.covariance) diff --git a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py b/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py deleted file mode 100644 index a828fdaa836..00000000000 --- a/dimos/msgs/geometry_msgs/TwistWithCovarianceStamped.py +++ /dev/null @@ -1,106 +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 time -from typing import TypeAlias - -from dimos_lcm.geometry_msgs import ( - TwistWithCovarianceStamped as LCMTwistWithCovarianceStamped, -) -import numpy as np - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.msgs.geometry_msgs.Vector3 import VectorConvertable -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from TwistWithCovarianceStamped -TwistWithCovarianceStampedConvertable: TypeAlias = ( - tuple[Twist | tuple[VectorConvertable, VectorConvertable], list[float] | np.ndarray] - | LCMTwistWithCovarianceStamped - | dict[ - str, - Twist - | tuple[VectorConvertable, VectorConvertable] - | list[float] - | np.ndarray - | float - | str, - ] -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class TwistWithCovarianceStamped(TwistWithCovariance, Timestamped): - msg_name = "geometry_msgs.TwistWithCovarianceStamped" - ts: float - frame_id: str - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - twist: Twist | tuple[VectorConvertable, VectorConvertable] | None = None, - covariance: list[float] | np.ndarray | None = None, - ) -> None: - """Initialize with timestamp, frame_id, twist and covariance.""" - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(twist, covariance) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMTwistWithCovarianceStamped() - lcm_msg.twist.twist = self.twist - # LCM expects list, not numpy array - if isinstance(self.covariance, np.ndarray): - lcm_msg.twist.covariance = self.covariance.tolist() - else: - lcm_msg.twist.covariance = list(self.covariance) - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - 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) -> TwistWithCovarianceStamped: - lcm_msg = LCMTwistWithCovarianceStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - twist=Twist( - linear=[ - lcm_msg.twist.twist.linear.x, - lcm_msg.twist.twist.linear.y, - lcm_msg.twist.twist.linear.z, - ], - angular=[ - lcm_msg.twist.twist.angular.x, - lcm_msg.twist.twist.angular.y, - lcm_msg.twist.twist.angular.z, - ], - ), - covariance=lcm_msg.twist.covariance, - ) - - def __str__(self) -> str: - return ( - f"TwistWithCovarianceStamped(linear=[{self.linear.x:.3f}, {self.linear.y:.3f}, {self.linear.z:.3f}], " - f"angular=[{self.angular.x:.3f}, {self.angular.y:.3f}, {self.angular.z:.3f}], " - f"cov_trace={np.trace(self.covariance_matrix):.3f})" - ) diff --git a/dimos/msgs/geometry_msgs/Vector3.py b/dimos/msgs/geometry_msgs/Vector3.py deleted file mode 100644 index 99c98677182..00000000000 --- a/dimos/msgs/geometry_msgs/Vector3.py +++ /dev/null @@ -1,434 +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 Sequence -from typing import Any, TypeAlias - -from dimos_lcm.geometry_msgs import Vector3 as LCMVector3 -import numpy as np - -# Types that can be converted to/from Vector -VectorConvertable: TypeAlias = Sequence[int | float] | LCMVector3 | np.ndarray - - -def _ensure_3d(data: np.ndarray) -> np.ndarray: - """Ensure the data array is exactly 3D by padding with zeros or raising an exception if too long.""" - if len(data) == 3: - return data - elif len(data) < 3: - padded = np.zeros(3, dtype=float) - padded[: len(data)] = data - return padded - else: - raise ValueError( - f"Vector3 cannot be initialized with more than 3 components. Got {len(data)} components." - ) - - -class Vector3(LCMVector3): # type: ignore[misc] - x: float = 0.0 - y: float = 0.0 - z: float = 0.0 - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a 3D vector. - - Supported forms: - Vector3() # zero vector - Vector3(x) # (x, 0, 0) - Vector3(x, y) # (x, y, 0) - Vector3(x, y, z) # (x, y, z) - Vector3(x=1, y=2, z=3) # keyword args - Vector3([x, y, z]) # sequence - Vector3(np.array([x, y, z])) # numpy array - Vector3(other_vector3) # copy constructor - Vector3(lcm_vector3) # from LCM message - """ - if kwargs and not args: - # Keyword arguments: Vector3(x=1, y=2, z=3) - self.x = float(kwargs.get("x", 0.0)) - self.y = float(kwargs.get("y", 0.0)) - self.z = float(kwargs.get("z", 0.0)) - elif not args: - # No arguments: zero vector - self.x = 0.0 - self.y = 0.0 - self.z = 0.0 - elif len(args) == 1: - arg = args[0] - if isinstance(arg, Vector3): - # Copy constructor - self.x = arg.x - self.y = arg.y - self.z = arg.z - elif isinstance(arg, LCMVector3): - # From LCM Vector3 - self.x = float(arg.x) - self.y = float(arg.y) - self.z = float(arg.z) - elif isinstance(arg, np.ndarray): - # From numpy array - data = _ensure_3d(np.array(arg, dtype=float)) - self.x = float(data[0]) - self.y = float(data[1]) - self.z = float(data[2]) - elif isinstance(arg, (list, tuple)): - # From sequence - data = _ensure_3d(np.array(arg, dtype=float)) - self.x = float(data[0]) - self.y = float(data[1]) - self.z = float(data[2]) - elif isinstance(arg, (int, float)): - # Single numeric value: (x, 0, 0) - self.x = float(arg) - self.y = 0.0 - self.z = 0.0 - else: - raise TypeError(f"Cannot initialize Vector3 from {type(arg)}") - elif len(args) == 2: - # Two numeric values: (x, y, 0) - self.x = float(args[0]) - self.y = float(args[1]) - self.z = 0.0 - elif len(args) == 3: - # Three numeric values: (x, y, z) - self.x = float(args[0]) - self.y = float(args[1]) - self.z = float(args[2]) - else: - raise TypeError(f"Vector3 takes at most 3 positional arguments ({len(args)} given)") - - @property - def as_tuple(self) -> tuple[float, float, float]: - return (self.x, self.y, self.z) - - @property - def yaw(self) -> float: - return self.z - - @property - def pitch(self) -> float: - return self.y - - @property - def roll(self) -> float: - return self.x - - @property - def data(self) -> np.ndarray: - """Get the underlying numpy array.""" - return np.array([self.x, self.y, self.z], dtype=float) - - def __getitem__(self, idx: int) -> float: - if idx == 0: - return self.x - elif idx == 1: - return self.y - elif idx == 2: - return self.z - else: - raise IndexError(f"Vector3 index {idx} out of range [0-2]") - - def __repr__(self) -> str: - return f"Vector({self.data})" - - def __str__(self) -> str: - def getArrow() -> str: - repr = ["←", "↖", "↑", "↗", "→", "↘", "↓", "↙"] - - if self.x == 0 and self.y == 0: - return "·" - - # Calculate angle in radians and convert to directional index - angle = np.arctan2(self.y, self.x) - # Map angle to 0-7 index (8 directions) with proper orientation - dir_index = int(((angle + np.pi) * 4 / np.pi) % 8) - # Get directional arrow symbol - return repr[dir_index] - - return f"{getArrow()} Vector {self.__repr__()}" - - def agent_encode(self) -> dict[str, float]: - """Encode the vector for agent communication.""" - return {"x": self.x, "y": self.y, "z": self.z} - - def serialize(self) -> dict[str, Any]: - """Serialize the vector to a tuple.""" - return {"type": "vector", "c": (self.x, self.y, self.z)} - - def __eq__(self, other: object) -> bool: - """Check if two vectors are equal using numpy's allclose for floating point comparison.""" - if not isinstance(other, Vector3): - return False - return bool(np.allclose([self.x, self.y, self.z], [other.x, other.y, other.z])) - - def __add__(self, other: VectorConvertable | Vector3) -> Vector3: - other_vector: Vector3 = to_vector(other) - return self.__class__( - self.x + other_vector.x, self.y + other_vector.y, self.z + other_vector.z - ) - - def __sub__(self, other: VectorConvertable | Vector3) -> Vector3: - other_vector = to_vector(other) - return self.__class__( - self.x - other_vector.x, self.y - other_vector.y, self.z - other_vector.z - ) - - def __mul__(self, scalar: float) -> Vector3: - return self.__class__(self.x * scalar, self.y * scalar, self.z * scalar) - - def __rmul__(self, scalar: float) -> Vector3: - return self.__mul__(scalar) - - def __truediv__(self, scalar: float) -> Vector3: - return self.__class__(self.x / scalar, self.y / scalar, self.z / scalar) - - def __neg__(self) -> Vector3: - return self.__class__(-self.x, -self.y, -self.z) - - def dot(self, other: VectorConvertable | Vector3) -> float: - """Compute dot product.""" - other_vector = to_vector(other) - return float(self.x * other_vector.x + self.y * other_vector.y + self.z * other_vector.z) - - def cross(self, other: VectorConvertable | Vector3) -> Vector3: - """Compute cross product (3D vectors only).""" - other_vector = to_vector(other) - return self.__class__( - self.y * other_vector.z - self.z * other_vector.y, - self.z * other_vector.x - self.x * other_vector.z, - self.x * other_vector.y - self.y * other_vector.x, - ) - - def magnitude(self) -> float: - """Alias for length().""" - return self.length() - - def length(self) -> float: - """Compute the Euclidean length (magnitude) of the vector.""" - return float(np.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)) - - def length_squared(self) -> float: - """Compute the squared length of the vector (faster than length()).""" - return float(self.x * self.x + self.y * self.y + self.z * self.z) - - def normalize(self) -> Vector3: - """Return a normalized unit vector in the same direction.""" - length = self.length() - if length < 1e-10: # Avoid division by near-zero - return self.__class__(0.0, 0.0, 0.0) - return self.__class__(self.x / length, self.y / length, self.z / length) - - def to_2d(self) -> Vector3: - """Convert a vector to a 2D vector by taking only the x and y components (z=0).""" - return self.__class__(self.x, self.y, 0.0) - - def distance(self, other: VectorConvertable | Vector3) -> float: - """Compute Euclidean distance to another vector.""" - other_vector = to_vector(other) - dx = self.x - other_vector.x - dy = self.y - other_vector.y - dz = self.z - other_vector.z - return float(np.sqrt(dx * dx + dy * dy + dz * dz)) - - def distance_squared(self, other: VectorConvertable | Vector3) -> float: - """Compute squared Euclidean distance to another vector (faster than distance()).""" - other_vector = to_vector(other) - dx = self.x - other_vector.x - dy = self.y - other_vector.y - dz = self.z - other_vector.z - return float(dx * dx + dy * dy + dz * dz) - - def angle(self, other: VectorConvertable | Vector3) -> float: - """Compute the angle (in radians) between this vector and another.""" - other_vector = to_vector(other) - this_length = self.length() - other_length = other_vector.length() - - if this_length < 1e-10 or other_length < 1e-10: - return 0.0 - - cos_angle = np.clip( - self.dot(other_vector) / (this_length * other_length), - -1.0, - 1.0, - ) - return float(np.arccos(cos_angle)) - - def project(self, onto: VectorConvertable | Vector3) -> Vector3: - """Project this vector onto another vector.""" - onto_vector = to_vector(onto) - onto_length_sq = ( - onto_vector.x * onto_vector.x - + onto_vector.y * onto_vector.y - + onto_vector.z * onto_vector.z - ) - if onto_length_sq < 1e-10: - return self.__class__(0.0, 0.0, 0.0) - - scalar_projection = self.dot(onto_vector) / onto_length_sq - return self.__class__( - scalar_projection * onto_vector.x, - scalar_projection * onto_vector.y, - scalar_projection * onto_vector.z, - ) - - @classmethod - def zeros(cls) -> Vector3: - """Create a zero 3D vector.""" - return cls() - - @classmethod - def ones(cls) -> Vector3: - """Create a 3D vector of ones.""" - return cls(1.0, 1.0, 1.0) - - @classmethod - def unit_x(cls) -> Vector3: - """Create a unit vector in the x direction.""" - return cls(1.0, 0.0, 0.0) - - @classmethod - def unit_y(cls) -> Vector3: - """Create a unit vector in the y direction.""" - return cls(0.0, 1.0, 0.0) - - @classmethod - def unit_z(cls) -> Vector3: - """Create a unit vector in the z direction.""" - return cls(0.0, 0.0, 1.0) - - def to_list(self) -> list[float]: - """Convert the vector to a list.""" - return [self.x, self.y, self.z] - - def to_tuple(self) -> tuple[float, float, float]: - """Convert the vector to a tuple.""" - return (self.x, self.y, self.z) - - def to_numpy(self) -> np.ndarray: - """Convert the vector to a numpy array.""" - return np.array([self.x, self.y, self.z], dtype=float) - - def is_zero(self) -> bool: - """Check if this is a zero vector (all components are zero). - - Returns: - True if all components are zero, False otherwise - """ - return bool(np.allclose([self.x, self.y, self.z], 0.0)) - - @property - def quaternion(self) -> Quaternion: # type: ignore[name-defined] - return self.to_quaternion() - - def to_quaternion(self) -> Quaternion: # type: ignore[name-defined] - """Convert Vector3 representing Euler angles (roll, pitch, yaw) to a Quaternion. - - Assumes this Vector3 contains Euler angles in radians: - - x component: roll (rotation around x-axis) - - y component: pitch (rotation around y-axis) - - z component: yaw (rotation around z-axis) - - Returns: - Quaternion: The equivalent quaternion representation - """ - # Import here to avoid circular imports - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - - # Extract Euler angles - roll = self.x - pitch = self.y - yaw = self.z - - # Convert Euler angles to quaternion using ZYX convention - # Source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles - - # Compute half angles - cy = np.cos(yaw * 0.5) - sy = np.sin(yaw * 0.5) - cp = np.cos(pitch * 0.5) - sp = np.sin(pitch * 0.5) - cr = np.cos(roll * 0.5) - sr = np.sin(roll * 0.5) - - # Compute quaternion components - w = cr * cp * cy + sr * sp * sy - x = sr * cp * cy - cr * sp * sy - y = cr * sp * cy + sr * cp * sy - z = cr * cp * sy - sr * sp * cy - - return Quaternion(x, y, z, w) - - def __bool__(self) -> bool: - """Boolean conversion for Vector. - - A Vector is considered False if it's a zero vector (all components are zero), - and True otherwise. - - Returns: - False if vector is zero, True otherwise - """ - return not self.is_zero() - - -def to_numpy(value: Vector3 | np.ndarray | Sequence[int | float]) -> np.ndarray: - """Convert a value to a numpy array.""" - if isinstance(value, Vector3): - return value.to_numpy() - elif isinstance(value, np.ndarray): - return value - else: - return np.array(value, dtype=float) - - -def to_vector(value: VectorConvertable | Vector3) -> Vector3: - """Convert a vector-compatible value to a Vector3 object.""" - if isinstance(value, Vector3): - return value - return Vector3(value) - - -def to_tuple(value: Vector3 | np.ndarray | Sequence[int | float]) -> tuple[float, ...]: - """Convert a value to a tuple.""" - if isinstance(value, Vector3): - return value.to_tuple() - elif isinstance(value, np.ndarray): - return tuple(value.tolist()) - elif isinstance(value, tuple): - return value - else: - return tuple(value) - - -def to_list(value: Vector3 | np.ndarray | Sequence[int | float]) -> list[float]: - """Convert a value to a list.""" - if isinstance(value, Vector3): - return value.to_list() - elif isinstance(value, np.ndarray): - result: list[float] = value.tolist() - return result - elif isinstance(value, list): - return value - else: - return list(value) - - -VectorLike: TypeAlias = VectorConvertable | Vector3 - - -def make_vector3(x: float, y: float, z: float) -> Vector3: - return Vector3(x, y, z) diff --git a/dimos/msgs/geometry_msgs/Wrench.py b/dimos/msgs/geometry_msgs/Wrench.py deleted file mode 100644 index 7b3a4427758..00000000000 --- a/dimos/msgs/geometry_msgs/Wrench.py +++ /dev/null @@ -1,121 +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 Sequence -from typing import Any, overload - -from dimos_lcm.geometry_msgs import Wrench as LCMWrench -import numpy as np - -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike - - -class Wrench(LCMWrench): # type: ignore[misc] - """Force (N) and torque (Nm) in 3D space.""" - - force: Vector3 - torque: Vector3 - msg_name = "geometry_msgs.Wrench" - - @overload - def __init__(self) -> None: ... - - @overload - def __init__( - self, - force: VectorLike | None = ..., - torque: VectorLike | None = ..., - ) -> None: ... - - @overload - def __init__(self, wrench: Wrench | LCMWrench, /) -> None: ... - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a wrench. - - Supported forms: - Wrench() # zero wrench - Wrench(force, torque) - Wrench(force=..., torque=...) # keyword args, either optional - Wrench(other_wrench) # copy constructor - Wrench(lcm_wrench) # from LCM message - """ - force: Any = None - torque: Any = None - - if len(args) == 2: - force, torque = args - elif len(args) == 1: - value = args[0] - # Wrench before LCMWrench (it is a subclass); anything else is a - # force, same as the `force=` keyword. - if isinstance(value, Wrench | LCMWrench): - force, torque = value.force, value.torque - else: - force = value - elif args: - raise TypeError(f"Wrench takes 1 or 2 positional arguments ({len(args)} given)") - - if kwargs: - force = kwargs.pop("force", force) - torque = kwargs.pop("torque", torque) - if kwargs: - raise TypeError(f"Wrench got unexpected keyword arguments {sorted(kwargs)}") - - self.force = Vector3() if force is None else Vector3(force) - self.torque = Vector3() if torque is None else Vector3(torque) - - @classmethod - def from_array(cls, ft: Sequence[float] | np.ndarray) -> Wrench: - """Build from a 6-element ``[fx, fy, fz, tx, ty, tz]`` sensor reading.""" - if len(ft) != 6: - raise ValueError(f"Expected 6 elements [fx, fy, fz, tx, ty, tz], got {len(ft)}") - return cls(force=ft[:3], torque=ft[3:]) - - def to_array(self) -> np.ndarray: - """``[fx, fy, fz, tx, ty, tz]``.""" - return np.concatenate([self.force.to_numpy(), self.torque.to_numpy()]) - - @classmethod - def zero(cls) -> Wrench: - return cls() - - def is_zero(self) -> bool: - return self.force.is_zero() and self.torque.is_zero() - - def __add__(self, other: Wrench) -> Wrench: - if not isinstance(other, Wrench): - return NotImplemented - return Wrench(force=self.force + other.force, torque=self.torque + other.torque) - - def __sub__(self, other: Wrench) -> Wrench: - if not isinstance(other, Wrench): - return NotImplemented - return Wrench(force=self.force - other.force, torque=self.torque - other.torque) - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Wrench): - return False - return self.force == other.force and self.torque == other.torque - - def __bool__(self) -> bool: - return not self.is_zero() - - def __repr__(self) -> str: - return f"Wrench(force={self.force!r}, torque={self.torque!r})" - - def __str__(self) -> str: - return f"Wrench:\n Force: {self.force}\n Torque: {self.torque}" diff --git a/dimos/msgs/geometry_msgs/WrenchStamped.py b/dimos/msgs/geometry_msgs/WrenchStamped.py deleted file mode 100644 index 12e6e940072..00000000000 --- a/dimos/msgs/geometry_msgs/WrenchStamped.py +++ /dev/null @@ -1,97 +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 Sequence -import time -from typing import Any, BinaryIO - -from dimos_lcm.geometry_msgs import WrenchStamped as LCMWrenchStamped -import numpy as np - -from dimos.msgs.geometry_msgs.Wrench import Wrench -from dimos.types.timestamped import Timestamped - - -class WrenchStamped(Wrench, Timestamped): - """A force/torque measurement with a timestamp and frame_id.""" - - msg_name = "geometry_msgs.WrenchStamped" - ts: float - frame_id: str - - def __init__(self, *args: Any, **kwargs: Any) -> None: - """Initialize a stamped wrench. - - Takes ``(ts, frame_id)`` positionally, plus every Wrench keyword. Any other - positional form belongs to Wrench, so ``WrenchStamped(force, torque)`` works - as it does on the base class. - """ - ts: Any - # A leading number or string is a timestamp; anything else is a Wrench argument. - if args and isinstance(args[0], int | float | str): - ts = args[0] - frame_id = args[1] if len(args) > 1 else kwargs.pop("frame_id", "") - wrench_args: tuple[Any, ...] = () - else: - ts = kwargs.pop("ts", None) - frame_id = kwargs.pop("frame_id", "") - wrench_args = args - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - super().__init__(*wrench_args, **kwargs) - - @classmethod - def from_array( # type: ignore[override] - cls, - ft: Sequence[float] | np.ndarray, - frame_id: str = "ft_sensor", - ts: float | None = None, - ) -> WrenchStamped: - """Build from a 6-element ``[fx, fy, fz, tx, ty, tz]`` sensor reading.""" - if len(ft) != 6: - raise ValueError(f"Expected 6 elements [fx, fy, fz, tx, ty, tz], got {len(ft)}") - return cls(ts=ts, frame_id=frame_id, force=ft[:3], torque=ft[3:]) - - def lcm_encode(self) -> bytes: - lcm_msg = LCMWrenchStamped() - lcm_msg.wrench = self - [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) -> WrenchStamped: - lcm_msg = LCMWrenchStamped.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - force=[lcm_msg.wrench.force.x, lcm_msg.wrench.force.y, lcm_msg.wrench.force.z], - torque=[lcm_msg.wrench.torque.x, lcm_msg.wrench.torque.y, lcm_msg.wrench.torque.z], - ) - - def __repr__(self) -> str: - return ( - f"WrenchStamped(force={self.force!r}, torque={self.torque!r}, " - f"ts={self.ts}, frame_id={self.frame_id!r})" - ) - - def __str__(self) -> str: - return ( - f"WrenchStamped(force=[{self.force.x:.3f}, {self.force.y:.3f}, {self.force.z:.3f}], " - f"torque=[{self.torque.x:.3f}, {self.torque.y:.3f}, {self.torque.z:.3f}], " - f"frame_id={self.frame_id!r})" - ) diff --git a/dimos/msgs/manipulation_msgs/GraspCandidate.py b/dimos/msgs/manipulation_msgs/GraspCandidate.py deleted file mode 100644 index f83db6016f4..00000000000 --- a/dimos/msgs/manipulation_msgs/GraspCandidate.py +++ /dev/null @@ -1,40 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -import math -import pickle - -from dimos.msgs.geometry_msgs.Pose import Pose - - -class GraspCandidate: - """A robot TCP pose and its generator-local ranking score.""" - - msg_name = "manipulation_msgs.GraspCandidate" - - def __init__(self, pose: Pose | None = None, score: float = 0.0) -> None: - self.pose = pose if pose is not None else Pose(0.0, 0.0, 0.0) - self.score = float(score) - if not math.isfinite(self.score): - raise ValueError("GraspCandidate.score must be finite") - - def encode(self) -> bytes: - return pickle.dumps({"pose": self.pose, "score": self.score}) - - @classmethod - def decode(cls, data: bytes) -> GraspCandidate: - value = pickle.loads(data) - return cls(value["pose"], value["score"]) diff --git a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py b/dimos/msgs/manipulation_msgs/GraspCandidateArray.py deleted file mode 100644 index 5144d0b5b91..00000000000 --- a/dimos/msgs/manipulation_msgs/GraspCandidateArray.py +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from __future__ import annotations - -from collections.abc import Iterator -import pickle - -from dimos.msgs.manipulation_msgs.GraspCandidate import GraspCandidate -from dimos.msgs.std_msgs.Header import Header - - -class GraspCandidateArray: - """Ordered grasp proposals sharing one input-cloud header.""" - - msg_name = "manipulation_msgs.GraspCandidateArray" - - def __init__( - self, header: Header | None = None, candidates: list[GraspCandidate] | None = None - ) -> None: - self.header = header if header is not None else Header(0.0) - self.candidates = candidates if candidates is not None else [] - - def __len__(self) -> int: - return len(self.candidates) - - def __iter__(self) -> Iterator[GraspCandidate]: - return iter(self.candidates) - - def encode(self) -> bytes: - """Encode using the repository's pickle transport convention.""" - return pickle.dumps({"header": self.header, "candidates": self.candidates}) - - @classmethod - def decode(cls, data: bytes) -> GraspCandidateArray: - value = pickle.loads(data) - return cls(value["header"], value["candidates"]) diff --git a/dimos/msgs/nav_msgs/LineSegments3D.py b/dimos/msgs/nav_msgs/LineSegments3D.py deleted file mode 100644 index ddb355efc5d..00000000000 --- a/dimos/msgs/nav_msgs/LineSegments3D.py +++ /dev/null @@ -1,130 +0,0 @@ -# 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. - -"""LineSegments3D: 3D line segments with a per-segment weight. - -On the wire uses ``nav_msgs/Path``. Consecutive pose pairs form segments and -``orientation.w`` carries the weight. -""" - -from __future__ import annotations - -import struct -import time -from typing import TYPE_CHECKING, BinaryIO - -import numpy as np - -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from numpy.typing import ArrayLike, NDArray - from rerun._baseclasses import Archetype - -# Path prefix after the 8 byte fingerprint: poses_length, header seq, stamp sec, stamp nsec, frame_id length. -_PREFIX = struct.Struct(">iiiiI") -# PoseStamped bytes before its frame_id text: header seq, stamp sec, stamp nsec, frame_id length. -_POSE_HEAD = 16 -_POSE_DOUBLES = 7 -_POSE_TAIL = _POSE_DOUBLES * 8 - - -class LineSegments3D(Timestamped): - """Line segments as an (N, 2, 3) array plus one weight per segment.""" - - msg_name = "nav_msgs.LineSegments3D" - ts: float - frame_id: str - segments: NDArray[np.float64] - weights: NDArray[np.float64] - - def __init__( - self, - ts: float | None = None, - frame_id: str = "map", - segments: ArrayLike | None = None, - weights: ArrayLike | None = None, - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - self.segments = np.asarray( - segments if segments is not None else np.empty((0, 2, 3)), dtype=np.float64 - ).reshape(-1, 2, 3) - self.weights = ( - np.ones(len(self.segments)) - if weights is None - else np.asarray(weights, dtype=np.float64).reshape(-1) - ) - - def lcm_encode(self) -> bytes: - raise NotImplementedError("Encoded on C++ side") - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> LineSegments3D: - """Read the Path payload through strided array views. - - Every pose header must carry the same frame_id length, which is what the planner emits. - """ - raw = data if isinstance(data, bytes) else data.read() - count, _, sec, nsec, frame_len = _PREFIX.unpack_from(raw, 8) - offset = 8 + _PREFIX.size - frame_id = raw[offset : offset + frame_len][:-1].decode("utf-8", "replace") - offset += frame_len - ts = sec + nsec / 1e9 - if count == 0: - return cls(ts=ts, frame_id=frame_id) - if count % 2: - raise ValueError(f"LineSegments3D needs pose pairs, got {count} poses") - (pose_frame_len,) = struct.unpack_from(">I", raw, offset + _POSE_HEAD - 4) - stride = _POSE_HEAD + pose_frame_len + _POSE_TAIL - lens = np.ndarray( - (count,), dtype=">u4", buffer=raw, offset=offset + _POSE_HEAD - 4, strides=(stride,) - ) - if len(raw) != offset + count * stride or not np.all(lens == pose_frame_len): - raise ValueError("LineSegments3D poses must share one frame_id length") - poses = np.ndarray( - (count, _POSE_DOUBLES), - dtype=">f8", - buffer=raw, - offset=offset + _POSE_HEAD + pose_frame_len, - strides=(stride, 8), - ) - return cls( - ts=ts, - frame_id=frame_id, - segments=poses[:, :3].astype(np.float64).reshape(-1, 2, 3), - weights=poses[0::2, 6].astype(np.float64), - ) - - def to_rerun(self, z_offset: float = 0.0, radii: float = 0.04) -> Archetype: - """Render as ``rr.LineStrips3D``, green to red by log-scale weight.""" - import rerun as rr - - if len(self.segments) == 0: - return rr.LineStrips3D([]) - strips = self.segments.astype(np.float32) - strips[:, :, 2] += z_offset - log_w = np.log10(np.maximum(self.weights, 1e-6)) - lo, hi = float(log_w.min()), float(log_w.max()) - norm = (log_w - lo) / (hi - lo) if hi > lo else np.zeros_like(log_w) - r = (255 * norm).astype(np.uint8) - g = (255 * (1.0 - norm)).astype(np.uint8) - colors = np.column_stack([r, g, np.full_like(r, 60), np.full_like(r, 220)]) - return rr.LineStrips3D(strips, colors=colors, radii=radii) - - def __len__(self) -> int: - return len(self.segments) - - def __str__(self) -> str: - return f"LineSegments3D(frame_id='{self.frame_id}', segments={len(self.segments)})" diff --git a/dimos/msgs/nav_msgs/OccupancyGrid.py b/dimos/msgs/nav_msgs/OccupancyGrid.py deleted file mode 100644 index 7370cd6bf70..00000000000 --- a/dimos/msgs/nav_msgs/OccupancyGrid.py +++ /dev/null @@ -1,598 +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 enum import IntEnum -from functools import lru_cache -import time -from typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.nav_msgs import ( - MapMetaData, - OccupancyGrid as LCMOccupancyGrid, -) -from dimos_lcm.std_msgs import Time as LCMTime -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Vector3 import Vector3, VectorLike -from dimos.types.timestamped import Timestamped - - -@lru_cache(maxsize=16) -def _get_matplotlib_cmap(name: str): # type: ignore[no-untyped-def] - """Get a matplotlib colormap by name (cached for performance).""" - # Lazy import to save time. - import matplotlib.pyplot as plt - - return plt.get_cmap(name) - - -@lru_cache(maxsize=16) -def _build_occupancy_lut( - colormap: str | None, - opacity: float, - background: str | None, -) -> np.ndarray: - """Build a 102-entry RGBA LUT for grid values -1..100. One-time cost per config.""" - bg_rgb = np.array([0, 0, 0], dtype=np.float32) - if background is not None: - bg = background.lstrip("#") - bg_rgb = np.array([int(bg[i : i + 2], 16) for i in (0, 2, 4)], dtype=np.float32) - - # LUT: index 0 = grid value -1 (unknown), index 1 = grid value 0 (free), index 2..101 = grid values 1..100 - lut = np.zeros((102, 4), dtype=np.uint8) - - # Unknown (-1) → index 0: black - lut[0] = [0, 0, 0, 255] - - if colormap is not None: - cmap = _get_matplotlib_cmap(colormap) - # Free (0) → index 1 - fg = np.array(cmap(0.0)[:3]) * 255 - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[1, :3] = blended.astype(np.uint8) - lut[1, 3] = 255 - - # Occupied (1..100) → index 2..101 - for cost in range(1, 101): - cost_norm = 0.5 + (cost / 100) * 0.5 - fg = np.array(cmap(cost_norm)[:3]) * 255 - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[cost + 1, :3] = blended.astype(np.uint8) - lut[cost + 1, 3] = 255 - else: - # Foxglove-style: free = #484981, occupied = gradient to black - fg_free = np.array([72, 73, 129], dtype=np.float32) - blended_free = fg_free * opacity + bg_rgb * (1 - opacity) - lut[1, :3] = blended_free.astype(np.uint8) - lut[1, 3] = 255 - - for cost in range(1, 101): - factor = 1 - cost / 100 - fg = np.array([72 * factor, 73 * factor, 129 * factor], dtype=np.float32) - blended = fg * opacity + bg_rgb * (1 - opacity) - lut[cost + 1, :3] = blended.astype(np.uint8) - lut[cost + 1, 3] = 255 - - return lut - - -if TYPE_CHECKING: - from pathlib import Path - - from numpy.typing import NDArray - from rerun._baseclasses import Archetype - - -def block_max_reduce(cells: NDArray[np.int8], factor: int) -> NDArray[np.int8]: - """Coarsen an occupancy grid by taking the max of factor x factor blocks. - - Block maximum (not mean) so coarsening never erases an obstacle; a block - is unknown (-1) only when every cell in it is unknown. Trailing remainder - rows/cols are trimmed, keeping row 0/col 0 - the origin corner - so the - grid origin stays valid. Grids thinner than `factor` pass through. - """ - h, w = cells.shape[:2] - new_h, new_w = h // factor, w // factor - if new_h == 0 or new_w == 0: - return cells - trimmed = cells[: new_h * factor, : new_w * factor] - blocks = trimmed.reshape(new_h, factor, new_w, factor) - # Sink unknown below every known value for the max, then map it back. - as_int = blocks.astype(np.int16) - known = np.where(as_int < 0, -1000, as_int) - reduced = known.max(axis=(1, 3)) - reduced[reduced == -1000] = -1 - result: NDArray[np.int8] = reduced.astype(np.int8) - return result - - -class CostValues(IntEnum): - """Standard cost values for occupancy grid cells. - - These values follow the ROS nav_msgs/OccupancyGrid convention: - - 0: Free space - - 1-99: Occupied space with varying cost levels - - 100: Lethal obstacle (definitely occupied) - - -1: Unknown space - """ - - UNKNOWN = -1 # Unknown space - FREE = 0 # Free space - OCCUPIED = 100 # Occupied/lethal space - - -class OccupancyGrid(Timestamped): - """ - Convenience wrapper for nav_msgs/OccupancyGrid with numpy array support. - """ - - msg_name = "nav_msgs.OccupancyGrid" - - # Attributes - ts: float - frame_id: str - info: MapMetaData - grid: NDArray[np.int8] - - def __init__( - self, - grid: NDArray[np.int8] | None = None, - width: int | None = None, - height: int | None = None, - resolution: float = 0.05, - origin: Pose | None = None, - frame_id: str = "world", - ts: float | None = None, - ) -> None: - """Initialize OccupancyGrid. - - Args: - grid: 2D numpy array of int8 values (height x width) - width: Width in cells (used if grid is None) - height: Height in cells (used if grid is None) - resolution: Grid resolution in meters/cell - origin: Origin pose of the grid - frame_id: Reference frame - ts: Timestamp (defaults to current time if 0) - """ - - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - - if grid is not None: - # Initialize from numpy array - if grid.ndim != 2: - raise ValueError("Grid must be a 2D array") - height, width = grid.shape - self.info = MapMetaData( - map_load_time=self._to_lcm_time(), # type: ignore[no-untyped-call] - resolution=resolution, - width=width, - height=height, - origin=origin or Pose(), - ) - self.grid = grid.astype(np.int8) - elif width is not None and height is not None: - # Initialize with dimensions - self.info = MapMetaData( - map_load_time=self._to_lcm_time(), # type: ignore[no-untyped-call] - resolution=resolution, - width=width, - height=height, - origin=origin or Pose(), - ) - self.grid = np.full((height, width), -1, dtype=np.int8) - else: - # Initialize empty - self.info = MapMetaData(map_load_time=self._to_lcm_time()) # type: ignore[no-untyped-call] - self.grid = np.array([], dtype=np.int8) - - def _to_lcm_time(self): # type: ignore[no-untyped-def] - """Convert timestamp to LCM Time.""" - - s = int(self.ts) - return LCMTime(sec=s, nsec=int((self.ts - s) * 1_000_000_000)) - - @property - def width(self) -> int: - """Width of the grid in cells.""" - return self.info.width # type: ignore[no-any-return] - - @property - def height(self) -> int: - """Height of the grid in cells.""" - return self.info.height # type: ignore[no-any-return] - - @property - def resolution(self) -> float: - """Grid resolution in meters/cell.""" - return self.info.resolution # type: ignore[no-any-return] - - @property - def origin(self) -> Pose: - """Origin pose of the grid.""" - return self.info.origin # type: ignore[no-any-return] - - @property - def total_cells(self) -> int: - """Total number of cells in the grid.""" - return self.width * self.height - - @property - def occupied_cells(self) -> int: - """Number of occupied cells (value >= 1).""" - return int(np.sum(self.grid >= 1)) - - @property - def free_cells(self) -> int: - """Number of free cells (value == 0).""" - return int(np.sum(self.grid == 0)) - - @property - def unknown_cells(self) -> int: - """Number of unknown cells (value == -1).""" - return int(np.sum(self.grid == -1)) - - @property - def occupied_percent(self) -> float: - """Percentage of cells that are occupied.""" - return (self.occupied_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @property - def free_percent(self) -> float: - """Percentage of cells that are free.""" - return (self.free_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @property - def unknown_percent(self) -> float: - """Percentage of cells that are unknown.""" - return (self.unknown_cells / self.total_cells * 100) if self.total_cells > 0 else 0.0 - - @classmethod - def from_path(cls, path: Path) -> OccupancyGrid: - match path.suffix.lower(): - case ".npy": - return cls(grid=np.load(path)) - case ".png": - # Lazy import: Pillow serves only this file loader; message - # transport must not depend on it. - from PIL import Image - - img = Image.open(path).convert("L") - return cls(grid=np.array(img).astype(np.int8)) - case _: - raise NotImplementedError(f"Unsupported file format: {path.suffix}") - - def world_to_grid(self, point: VectorLike) -> Vector3: - """Convert world coordinates to grid coordinates. - - Args: - point: A vector-like object containing X,Y coordinates - - Returns: - Vector3 with grid coordinates - """ - positionVector = Vector3(point) - # Get origin position - ox = self.origin.position.x - oy = self.origin.position.y - - # Convert to grid coordinates (simplified, assuming no rotation) - grid_x = (positionVector.x - ox) / self.resolution - grid_y = (positionVector.y - oy) / self.resolution - - return Vector3(grid_x, grid_y, 0.0) - - def grid_to_world(self, grid_point: VectorLike) -> Vector3: - """Convert grid coordinates to world coordinates. - - Args: - grid_point: Vector-like object containing grid coordinates - - Returns: - World position as Vector3 - """ - gridVector = Vector3(grid_point) - # Get origin position - ox = self.origin.position.x - oy = self.origin.position.y - - # Convert to world (simplified, no rotation) - x = ox + gridVector.x * self.resolution - y = oy + gridVector.y * self.resolution - - return Vector3(x, y, 0.0) - - def __str__(self) -> str: - """Create a concise string representation.""" - origin_pos = self.origin.position - - parts = [ - f"▦ OccupancyGrid[{self.frame_id}]", - f"{self.width}x{self.height}", - f"({self.width * self.resolution:.1f}x{self.height * self.resolution:.1f}m @", - f"{1 / self.resolution:.0f}cm res)", - f"Origin: ({origin_pos.x:.2f}, {origin_pos.y:.2f})", - f"▣ {self.occupied_percent:.1f}%", - f"□ {self.free_percent:.1f}%", - f"◌ {self.unknown_percent:.1f}%", - ] - - return " ".join(parts) - - def __repr__(self) -> str: - """Create a detailed representation.""" - return ( - f"OccupancyGrid(width={self.width}, height={self.height}, " - f"resolution={self.resolution}, frame_id='{self.frame_id}', " - f"occupied={self.occupied_cells}, free={self.free_cells}, " - f"unknown={self.unknown_cells})" - ) - - def lcm_encode(self) -> bytes: - """Encode OccupancyGrid to LCM bytes.""" - # Create LCM message - lcm_msg = LCMOccupancyGrid() - - # Build header on demand - s = int(self.ts) - lcm_msg.header.stamp.sec = s - lcm_msg.header.stamp.nsec = int((self.ts - s) * 1_000_000_000) - lcm_msg.header.frame_id = self.frame_id - - # Copy map metadata - lcm_msg.info = self.info - - # Convert numpy array to flat data list - if self.grid.size > 0: - flat_data = self.grid.flatten() - lcm_msg.data_length = len(flat_data) - lcm_msg.data = flat_data.tolist() - else: - lcm_msg.data_length = 0 - lcm_msg.data = [] - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> OccupancyGrid: - """Decode LCM bytes to OccupancyGrid.""" - lcm_msg = LCMOccupancyGrid.lcm_decode(data) - - # Extract timestamp and frame_id from header - ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - frame_id = lcm_msg.header.frame_id - - # Extract grid data; empty stays 2-D so the constructor accepts it. - if lcm_msg.data and lcm_msg.info.width > 0 and lcm_msg.info.height > 0: - grid = np.array(lcm_msg.data, dtype=np.int8).reshape( - (lcm_msg.info.height, lcm_msg.info.width) - ) - else: - grid = np.zeros((0, 0), dtype=np.int8) - - # The wire origin decodes as a plain generated pose without the dimos - # Pose surface (.yaw etc.); rebuild it field by field so the origin - # property honors its declared type. - o = lcm_msg.info.origin - lcm_msg.info.origin = Pose( - position=[o.position.x, o.position.y, o.position.z], - orientation=[o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w], - ) - - # Create new instance - instance = cls( - grid=grid, - resolution=lcm_msg.info.resolution, - origin=lcm_msg.info.origin, - frame_id=frame_id, - ts=ts, - ) - instance.info = lcm_msg.info - return instance - - def filter_above(self, threshold: int) -> OccupancyGrid: - """Create a new OccupancyGrid with only values above threshold. - - Args: - threshold: Keep cells with values > threshold - - Returns: - New OccupancyGrid where: - - Cells > threshold: kept as-is - - Cells <= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and <= threshold) - filter_mask = (new_grid != -1) & (new_grid <= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def filter_below(self, threshold: int) -> OccupancyGrid: - """Create a new OccupancyGrid with only values below threshold. - - Args: - threshold: Keep cells with values < threshold - - Returns: - New OccupancyGrid where: - - Cells < threshold: kept as-is - - Cells >= threshold: set to -1 (unknown) - - Unknown cells (-1): preserved - """ - new_grid = self.grid.copy() - - # Create mask for cells to filter (not unknown and >= threshold) - filter_mask = (new_grid != -1) & (new_grid >= threshold) - - # Set filtered cells to unknown - new_grid[filter_mask] = -1 - - # Create new OccupancyGrid - filtered = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return filtered - - def max(self) -> OccupancyGrid: - """Create a new OccupancyGrid with all non-unknown cells set to maximum value. - - Returns: - New OccupancyGrid where: - - All non-unknown cells: set to CostValues.OCCUPIED (100) - - Unknown cells: preserved as CostValues.UNKNOWN (-1) - """ - new_grid = self.grid.copy() - - # Set all non-unknown cells to max - new_grid[new_grid != CostValues.UNKNOWN] = CostValues.OCCUPIED - - # Create new OccupancyGrid - maxed = OccupancyGrid( - new_grid, - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - return maxed - - def copy(self) -> OccupancyGrid: - """Create a deep copy of the OccupancyGrid. - - Returns: - A new OccupancyGrid instance with copied data. - """ - return OccupancyGrid( - grid=self.grid.copy(), - resolution=self.resolution, - origin=self.origin, - frame_id=self.frame_id, - ts=self.ts, - ) - - def cell_value(self, world_position: Vector3) -> int: - grid_position = self.world_to_grid(world_position) - x = int(grid_position.x) - y = int(grid_position.y) - - if not (0 <= x < self.width and 0 <= y < self.height): - return CostValues.UNKNOWN - - return int(self.grid[y, x]) - - def to_rerun( - self, - colormap: str | None = None, - z_offset: float = 0, - opacity: float = 1.0, - cost_range: tuple[int, int] | None = None, - background: str | None = None, - color_lookup_table: np.ndarray | None = None, - ) -> Archetype: - """Convert to 3D textured mesh overlay on floor plane. - - Uses a single quad with the occupancy grid as a texture. - Much more efficient than per-cell quads (4 vertices vs n_cells*4). - """ - import rerun as rr - - if self.grid.size == 0: - return rr.Mesh3D(vertex_positions=[]) - - # Downsample grid before LUT to avoid allocating huge RGBA array - max_tex = 256 - grid = self.grid[::-1] # flip for world coords (view, no copy) - if grid.shape[0] > max_tex or grid.shape[1] > max_tex: - step_h = max(1, grid.shape[0] // max_tex) - step_w = max(1, grid.shape[1] // max_tex) - grid = grid[::step_h, ::step_w] - - if color_lookup_table is None: - color_lookup_table = _build_occupancy_lut(colormap, opacity, background) - rgba = np.ascontiguousarray(color_lookup_table[np.clip(grid + 1, 0, 101)]) - - # Apply cost_range filter on downsampled grid - if cost_range is not None: - out_of_range = ((grid < cost_range[0]) | (grid > cost_range[1])) & (grid != -1) - if np.any(out_of_range): - bg_rgb = [0, 0, 0] - if background is not None: - bg = background.lstrip("#") - bg_rgb = [int(bg[i : i + 2], 16) for i in (0, 2, 4)] - rgba[out_of_range, :3] = bg_rgb - rgba[out_of_range, 3] = 255 - - # Single quad covering entire grid - ox = self.origin.position.x - oy = self.origin.position.y - w = self.width * self.resolution - h = self.height * self.resolution - - vertices = np.array( - [ - [ox, oy, z_offset], # 0: bottom-left (world) - [ox + w, oy, z_offset], # 1: bottom-right - [ox + w, oy + h, z_offset], # 2: top-right - [ox, oy + h, z_offset], # 3: top-left - ], - dtype=np.float32, - ) - - indices = np.array([[0, 1, 2], [0, 2, 3]], dtype=np.uint32) - - # UV coords: Rerun uses top-left origin for textures - # Grid row 0 is at world y=oy (bottom), row H-1 at y=oy+h (top) - # Texture row 0 = grid row 0, so: - # world bottom (v0,v1) -> texture v=1 (bottom of texture) - # world top (v2,v3) -> texture v=0 (top of texture) - texcoords = np.array( - [ - [0.0, 1.0], # v0: bottom-left world -> bottom-left tex - [1.0, 1.0], # v1: bottom-right world -> bottom-right tex - [1.0, 0.0], # v2: top-right world -> top-right tex - [0.0, 0.0], # v3: top-left world -> top-left tex - ], - dtype=np.float32, - ) - - return rr.Mesh3D( - vertex_positions=vertices, - triangle_indices=indices, - vertex_texcoords=texcoords, - albedo_texture=rgba, - ) diff --git a/dimos/msgs/nav_msgs/Odometry.py b/dimos/msgs/nav_msgs/Odometry.py deleted file mode 100644 index 5e986672ea9..00000000000 --- a/dimos/msgs/nav_msgs/Odometry.py +++ /dev/null @@ -1,249 +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 time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos_lcm.nav_msgs import Odometry as LCMOdometry -import numpy as np - -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.PoseWithCovariance import PoseWithCovariance -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.TwistWithCovariance import TwistWithCovariance -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - from dimos.msgs.geometry_msgs.Vector3 import Vector3 - - -class Odometry(Timestamped): - """Odometry message with pose, twist, and frame information.""" - - msg_name = "nav_msgs.Odometry" - - def __init__( - self, - ts: float | None = None, - frame_id: str = "", - child_frame_id: str = "", - pose: PoseWithCovariance | Pose | None = None, - twist: TwistWithCovariance | Twist | None = None, - ) -> None: - self.ts = time.time() if ts is None else ts - self.frame_id = frame_id - self.child_frame_id = child_frame_id - - if pose is None: - self.pose = PoseWithCovariance() - elif isinstance(pose, Pose): - self.pose = PoseWithCovariance(pose) - else: - self.pose = pose - - if twist is None: - self.twist = TwistWithCovariance() - elif isinstance(twist, Twist): - self.twist = TwistWithCovariance(twist) - else: - self.twist = twist - - # -- Convenience properties -- - - @property - def position(self) -> Vector3: - return self.pose.position - - @property - def orientation(self) -> Quaternion: - return self.pose.orientation - - @property - def linear_velocity(self) -> Vector3: - return self.twist.linear - - @property - def angular_velocity(self) -> Vector3: - return self.twist.angular - - @property - def x(self) -> float: - return self.pose.x - - @property - def y(self) -> float: - return self.pose.y - - @property - def z(self) -> float: - return self.pose.z - - @property - def vx(self) -> float: - return self.twist.linear.x - - @property - def vy(self) -> float: - return self.twist.linear.y - - @property - def vz(self) -> float: - return self.twist.linear.z - - @property - def wx(self) -> float: - return self.twist.angular.x - - @property - def wy(self) -> float: - return self.twist.angular.y - - @property - def wz(self) -> float: - return self.twist.angular.z - - @property - def roll(self) -> float: - return self.pose.roll - - @property - def pitch(self) -> float: - return self.pose.pitch - - @property - def yaw(self) -> float: - return self.pose.yaw - - def to_pose_stamped(self) -> PoseStamped: - return PoseStamped( - ts=self.ts, - frame_id=self.frame_id, - position=[self.position.x, self.position.y, self.position.z], - orientation=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ], - ) - - # -- Serialization -- - - def lcm_encode(self) -> bytes: - lcm_msg = LCMOdometry() - - lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec = self.ros_timestamp() - lcm_msg.header.frame_id = self.frame_id - lcm_msg.child_frame_id = self.child_frame_id - - lcm_msg.pose.pose = self.pose.pose - lcm_msg.pose.covariance = list(np.asarray(self.pose.covariance)) - - lcm_msg.twist.twist = self.twist.twist - lcm_msg.twist.covariance = list(np.asarray(self.twist.covariance)) - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Odometry: - lcm_msg = LCMOdometry.lcm_decode(data) - - ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - - pose = Pose( - position=[ - lcm_msg.pose.pose.position.x, - lcm_msg.pose.pose.position.y, - lcm_msg.pose.pose.position.z, - ], - orientation=[ - lcm_msg.pose.pose.orientation.x, - lcm_msg.pose.pose.orientation.y, - lcm_msg.pose.pose.orientation.z, - lcm_msg.pose.pose.orientation.w, - ], - ) - twist = Twist( - linear=[ - lcm_msg.twist.twist.linear.x, - lcm_msg.twist.twist.linear.y, - lcm_msg.twist.twist.linear.z, - ], - angular=[ - lcm_msg.twist.twist.angular.x, - lcm_msg.twist.twist.angular.y, - lcm_msg.twist.twist.angular.z, - ], - ) - - return cls( - ts=ts, - frame_id=lcm_msg.header.frame_id, - child_frame_id=lcm_msg.child_frame_id, - pose=PoseWithCovariance(pose, lcm_msg.pose.covariance), - twist=TwistWithCovariance(twist, lcm_msg.twist.covariance), - ) - - # -- Comparison / display -- - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Odometry): - return False - return ( - abs(self.ts - other.ts) < 1e-6 - and self.frame_id == other.frame_id - and self.child_frame_id == other.child_frame_id - and self.pose == other.pose - and self.twist == other.twist - ) - - def __repr__(self) -> str: - return ( - f"Odometry(ts={self.ts:.6f}, frame_id='{self.frame_id}', " - f"child_frame_id='{self.child_frame_id}', pose={self.pose!r}, twist={self.twist!r})" - ) - - def __str__(self) -> str: - return ( - f"Odometry:\n" - f" Timestamp: {self.ts:.6f}\n" - f" Frame: {self.frame_id} -> {self.child_frame_id}\n" - f" Position: [{self.x:.3f}, {self.y:.3f}, {self.z:.3f}]\n" - f" Orientation: [roll={self.roll:.3f}, pitch={self.pitch:.3f}, yaw={self.yaw:.3f}]\n" - f" Linear Velocity: [{self.vx:.3f}, {self.vy:.3f}, {self.vz:.3f}]\n" - f" Angular Velocity: [{self.wx:.3f}, {self.wy:.3f}, {self.wz:.3f}]" - ) - - def to_rerun(self) -> Archetype: - """Convert to rerun Transform3D for visualizing the pose.""" - import rerun as rr - - return rr.Transform3D( - translation=[self.x, self.y, self.z], - rotation=rr.Quaternion( - xyzw=[ - self.orientation.x, - self.orientation.y, - self.orientation.z, - self.orientation.w, - ] - ), - ) diff --git a/dimos/msgs/nav_msgs/Path.py b/dimos/msgs/nav_msgs/Path.py deleted file mode 100644 index cd901f2a06f..00000000000 --- a/dimos/msgs/nav_msgs/Path.py +++ /dev/null @@ -1,214 +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 time -from typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.geometry_msgs import ( - Point as LCMPoint, - Pose as LCMPose, - PoseStamped as LCMPoseStamped, - Quaternion as LCMQuaternion, -) -from dimos_lcm.nav_msgs import Path as LCMPath -from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - from collections.abc import Iterator - - from rerun._baseclasses import Archetype - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -class Path(Timestamped): - msg_name = "nav_msgs.Path" - ts: float - frame_id: str - poses: list[PoseStamped] - - def __init__( # type: ignore[no-untyped-def] - self, - ts: float | None = None, - frame_id: str = "world", - poses: list[PoseStamped] | None = None, - **kwargs, - ) -> None: - self.frame_id = frame_id - self.ts = time.time() if ts is None else ts - self.poses = poses if poses is not None else [] - - def __len__(self) -> int: - """Return the number of poses in the path.""" - return len(self.poses) - - def __bool__(self) -> bool: - """Return True if path has poses.""" - return len(self.poses) > 0 - - def head(self) -> PoseStamped | None: - """Return the first pose in the path, or None if empty.""" - return self.poses[0] if self.poses else None - - def last(self) -> PoseStamped | None: - """Return the last pose in the path, or None if empty.""" - return self.poses[-1] if self.poses else None - - def tail(self) -> Path: - """Return a new Path with all poses except the first.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses[1:] if self.poses else []) - - def push(self, pose: PoseStamped) -> Path: - """Return a new Path with the pose appended (immutable).""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=[*self.poses, pose]) - - def push_mut(self, pose: PoseStamped) -> None: - """Append a pose to this path (mutable).""" - self.poses.append(pose) - - def lcm_encode(self) -> bytes: - """Encode Path to LCM bytes.""" - lcm_msg = LCMPath() - - # Set poses - lcm_msg.poses_length = len(self.poses) - lcm_poses = [] # Build list separately to avoid LCM library reuse issues - for pose in self.poses: - lcm_pose = LCMPoseStamped() - # Create new pose objects to avoid LCM library reuse bug - lcm_pose.pose = LCMPose() - lcm_pose.pose.position = LCMPoint() - lcm_pose.pose.orientation = LCMQuaternion() - - # Set the pose geometry data - lcm_pose.pose.position.x = pose.x - lcm_pose.pose.position.y = pose.y - lcm_pose.pose.position.z = pose.z - lcm_pose.pose.orientation.x = pose.orientation.x - lcm_pose.pose.orientation.y = pose.orientation.y - lcm_pose.pose.orientation.z = pose.orientation.z - lcm_pose.pose.orientation.w = pose.orientation.w - - # Create new header to avoid reuse - lcm_pose.header = LCMHeader() - lcm_pose.header.stamp = LCMTime() - - # Set the header with pose timestamp but path's frame_id - [lcm_pose.header.stamp.sec, lcm_pose.header.stamp.nsec] = sec_nsec(pose.ts) # type: ignore[no-untyped-call] - lcm_pose.header.frame_id = self.frame_id # All poses use path's frame_id - lcm_poses.append(lcm_pose) - lcm_msg.poses = lcm_poses - - # Set header with path's own timestamp - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - 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) -> Path: - """Decode LCM bytes to Path.""" - lcm_msg = LCMPath.lcm_decode(data) - - # Decode header - header_ts = lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000) - frame_id = lcm_msg.header.frame_id - - # Decode poses - all use the path's frame_id - poses = [] - for lcm_pose in lcm_msg.poses: - pose = PoseStamped( - ts=lcm_pose.header.stamp.sec + (lcm_pose.header.stamp.nsec / 1_000_000_000), - frame_id=frame_id, # Use path's frame_id for all poses - position=[ - lcm_pose.pose.position.x, - lcm_pose.pose.position.y, - lcm_pose.pose.position.z, - ], - orientation=[ - lcm_pose.pose.orientation.x, - lcm_pose.pose.orientation.y, - lcm_pose.pose.orientation.z, - lcm_pose.pose.orientation.w, - ], - ) - poses.append(pose) - - # Use header timestamp for the path - return cls(ts=header_ts, frame_id=frame_id, poses=poses) - - def __str__(self) -> str: - """String representation of Path.""" - return f"Path(frame_id='{self.frame_id}', poses={len(self.poses)})" - - def __getitem__(self, index: int | slice) -> PoseStamped | list[PoseStamped]: - """Allow indexing and slicing of poses.""" - return self.poses[index] - - def __iter__(self) -> Iterator: # type: ignore[type-arg] - """Allow iteration over poses.""" - return iter(self.poses) - - def slice(self, start: int, end: int | None = None) -> Path: - """Return a new Path with a slice of poses.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses[start:end]) - - def extend(self, other: Path) -> Path: - """Return a new Path with poses from both paths (immutable).""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=self.poses + other.poses) - - def extend_mut(self, other: Path) -> None: - """Extend this path with poses from another path (mutable).""" - self.poses.extend(other.poses) - - def reverse(self) -> Path: - """Return a new Path with poses in reverse order.""" - return Path(ts=self.ts, frame_id=self.frame_id, poses=list(reversed(self.poses))) - - def clear(self) -> None: - """Clear all poses from this path (mutable).""" - self.poses.clear() - - def to_rerun( - self, - color: tuple[int, int, int] = (0, 255, 128), - z_offset: float = 0.5, - radii: float = 0.05, - ) -> Archetype: - """Convert to rerun LineStrips3D format. - - Args: - color: RGB color tuple for the path line - z_offset: Height above floor to render path (default 0.2m to avoid costmap occlusion) - radii: Thickness of the path line (default 0.05m = 5cm) - - Returns: - rr.LineStrips3D archetype for logging to rerun - """ - import rerun as rr - - if not self.poses: - return rr.LineStrips3D([]) - - # Lift path above floor so it's visible over costmap - points = [[p.x, p.y, p.z + z_offset] for p in self.poses] - return rr.LineStrips3D([points], colors=[color], radii=radii) diff --git a/dimos/msgs/sensor_msgs/CameraInfo.py b/dimos/msgs/sensor_msgs/CameraInfo.py deleted file mode 100644 index ac2d38032f0..00000000000 --- a/dimos/msgs/sensor_msgs/CameraInfo.py +++ /dev/null @@ -1,539 +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 math -import time -from typing import TYPE_CHECKING, Any - -if TYPE_CHECKING: - from dimos.visualization.rerun.bridge import RerunData - -# Import LCM types -from dimos_lcm.sensor_msgs import CameraInfo as LCMCameraInfo -from dimos_lcm.std_msgs.Header import Header -import numpy as np - -from dimos.types.timestamped import Timestamped - - -class CameraInfo(Timestamped): - """Camera calibration information message.""" - - msg_name = "sensor_msgs.CameraInfo" - - def __init__( - self, - height: int = 0, - width: int = 0, - distortion_model: str = "", - D: list[float] | None = None, - K: list[float] | None = None, - R: list[float] | None = None, - P: list[float] | None = None, - binning_x: int = 0, - binning_y: int = 0, - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize CameraInfo. - - Args: - height: Image height - width: Image width - distortion_model: Name of distortion model (e.g., "plumb_bob") - D: Distortion coefficients - K: 3x3 intrinsic camera matrix - R: 3x3 rectification matrix - P: 3x4 projection matrix - binning_x: Horizontal binning - binning_y: Vertical binning - frame_id: Frame ID - ts: Timestamp - """ - self.ts = ts if ts is not None else time.time() - self.frame_id = frame_id - self.height = height - self.width = width - self.distortion_model = distortion_model - - # Initialize distortion coefficients - self.D = D if D is not None else [] - - # Initialize 3x3 intrinsic camera matrix (row-major) - self.K = K if K is not None else [0.0] * 9 - - # Initialize 3x3 rectification matrix (row-major) - self.R = R if R is not None else [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - - # Initialize 3x4 projection matrix (row-major) - self.P = P if P is not None else [0.0] * 12 - - self.binning_x = binning_x - self.binning_y = binning_y - - # Region of interest (not used in basic implementation) - self.roi_x_offset = 0 - self.roi_y_offset = 0 - self.roi_height = 0 - self.roi_width = 0 - self.roi_do_rectify = False - - @classmethod - def from_intrinsics( - cls, - fx: float, - fy: float, - cx: float, - cy: float, - width: int, - height: int, - frame_id: str = "", - ) -> CameraInfo: - """Create CameraInfo from pinhole intrinsics (no distortion). - - Args: - fx: Focal length x (pixels) - fy: Focal length y (pixels) - cx: Principal point x (pixels) - cy: Principal point y (pixels) - width: Image width - height: Image height - frame_id: Frame ID - """ - return cls( - height=height, - width=width, - distortion_model="plumb_bob", - D=[0.0, 0.0, 0.0, 0.0, 0.0], - K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], - P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], - frame_id=frame_id, - ) - - @classmethod - def from_fov( - cls, - fov_deg: float, - width: int, - height: int, - axis: str = "vertical", - frame_id: str = "", - ) -> CameraInfo: - """Create CameraInfo from field of view and image dimensions. - - Args: - fov_deg: Field of view in degrees along ``axis`` - width: Image width (pixels) - height: Image height (pixels) - axis: Which image axis ``fov_deg`` refers to ("vertical" or "horizontal") - frame_id: Frame ID - """ - fov_rad = math.radians(fov_deg) - if axis == "vertical": - f = (height / 2) / math.tan(fov_rad / 2) - elif axis == "horizontal": - f = (width / 2) / math.tan(fov_rad / 2) - else: - raise ValueError(f"axis must be 'vertical' or 'horizontal', got {axis!r}") - return cls.from_intrinsics( - fx=f, - fy=f, - cx=width / 2.0, - cy=height / 2.0, - width=width, - height=height, - frame_id=frame_id, - ) - - def with_ts(self, ts: float) -> CameraInfo: - """Return a copy of this CameraInfo with the given timestamp. - - Args: - ts: New timestamp - - Returns: - New CameraInfo instance with updated timestamp - """ - return CameraInfo( - height=self.height, - width=self.width, - distortion_model=self.distortion_model, - D=self.D.copy(), - K=self.K.copy(), - R=self.R.copy(), - P=self.P.copy(), - binning_x=self.binning_x, - binning_y=self.binning_y, - frame_id=self.frame_id, - ts=ts, - ) - - @classmethod - def from_yaml(cls, yaml_file: str, frame_id: str = "camera_optical") -> CameraInfo: - """Create CameraInfo from YAML file. - - Args: - yaml_file: Path to YAML file containing camera calibration data - frame_id: Optical frame the intrinsics apply to; must match the - ``Image.frame_id`` published for that camera - - Returns: - CameraInfo instance with loaded calibration data - """ - import yaml - - with open(yaml_file) as f: - data = yaml.safe_load(f) - - # Extract basic parameters - width = data.get("image_width", 0) - height = data.get("image_height", 0) - distortion_model = data.get("distortion_model", "") - - # Extract matrices - camera_matrix = data.get("camera_matrix", {}) - K = camera_matrix.get("data", [0.0] * 9) - - distortion_coeffs = data.get("distortion_coefficients", {}) - D = distortion_coeffs.get("data", []) - - rect_matrix = data.get("rectification_matrix", {}) - R = rect_matrix.get("data", [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]) - - proj_matrix = data.get("projection_matrix", {}) - P = proj_matrix.get("data", [0.0] * 12) - - # Create CameraInfo instance - return cls( - height=height, - width=width, - distortion_model=distortion_model, - D=D, - K=K, - R=R, - P=P, - frame_id=frame_id, - ) - - def get_K_matrix(self) -> np.ndarray: - """Get intrinsic matrix as numpy array.""" - return np.array(self.K, dtype=np.float64).reshape(3, 3) - - def get_P_matrix(self) -> np.ndarray: - """Get projection matrix as numpy array.""" - return np.array(self.P, dtype=np.float64).reshape(3, 4) - - def get_R_matrix(self) -> np.ndarray: - """Get rectification matrix as numpy array.""" - return np.array(self.R, dtype=np.float64).reshape(3, 3) - - def get_D_coeffs(self) -> np.ndarray: - """Get distortion coefficients as numpy array.""" - return np.array(self.D, dtype=np.float64) - - def set_K_matrix(self, K: np.ndarray): # type: ignore[no-untyped-def] - """Set intrinsic matrix from numpy array.""" - if K.shape != (3, 3): - raise ValueError(f"K matrix must be 3x3, got {K.shape}") - self.K = K.flatten().tolist() - - def set_P_matrix(self, P: np.ndarray): # type: ignore[no-untyped-def] - """Set projection matrix from numpy array.""" - if P.shape != (3, 4): - raise ValueError(f"P matrix must be 3x4, got {P.shape}") - self.P = P.flatten().tolist() - - def set_R_matrix(self, R: np.ndarray): # type: ignore[no-untyped-def] - """Set rectification matrix from numpy array.""" - if R.shape != (3, 3): - raise ValueError(f"R matrix must be 3x3, got {R.shape}") - self.R = R.flatten().tolist() - - def set_D_coeffs(self, D: np.ndarray) -> None: - """Set distortion coefficients from numpy array.""" - self.D = D.flatten().tolist() - - def lcm_encode(self) -> bytes: - """Convert to LCM CameraInfo message.""" - msg = LCMCameraInfo() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = self.frame_id - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - # Image dimensions - msg.height = self.height - msg.width = self.width - - # Distortion model - msg.distortion_model = self.distortion_model - - # Distortion coefficients - msg.D_length = len(self.D) - msg.D = self.D - - # Camera matrices (all stored as row-major) - msg.K = self.K - msg.R = self.R - msg.P = self.P - - # Binning - msg.binning_x = self.binning_x - msg.binning_y = self.binning_y - - # ROI - msg.roi.x_offset = self.roi_x_offset - msg.roi.y_offset = self.roi_y_offset - msg.roi.height = self.roi_height - msg.roi.width = self.roi_width - msg.roi.do_rectify = self.roi_do_rectify - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> CameraInfo: - """Decode from LCM CameraInfo bytes.""" - msg = LCMCameraInfo.lcm_decode(data) - - # Extract timestamp - ts = msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 if hasattr(msg, "header") else None - - camera_info = cls( - height=msg.height, - width=msg.width, - distortion_model=msg.distortion_model, - D=list(msg.D) if msg.D_length > 0 else [], - K=list(msg.K), - R=list(msg.R), - P=list(msg.P), - binning_x=msg.binning_x, - binning_y=msg.binning_y, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=ts, - ) - - # Set ROI if present - if hasattr(msg, "roi"): - camera_info.roi_x_offset = msg.roi.x_offset - camera_info.roi_y_offset = msg.roi.y_offset - camera_info.roi_height = msg.roi.height - camera_info.roi_width = msg.roi.width - camera_info.roi_do_rectify = msg.roi.do_rectify - - return camera_info - - def __repr__(self) -> str: - """String representation.""" - return ( - f"CameraInfo(height={self.height}, width={self.width}, " - f"distortion_model='{self.distortion_model}', " - f"frame_id='{self.frame_id}', ts={self.ts})" - ) - - def __str__(self) -> str: - """Human-readable string.""" - return ( - f"CameraInfo:\n" - f" Resolution: {self.width}x{self.height}\n" - f" Distortion model: {self.distortion_model}\n" - f" Frame ID: {self.frame_id}\n" - f" Binning: {self.binning_x}x{self.binning_y}" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two CameraInfo messages are equal.""" - if not isinstance(other, CameraInfo): - return False - - return ( - self.height == other.height - and self.width == other.width - and self.distortion_model == other.distortion_model - and self.D == other.D - and self.K == other.K - and self.R == other.R - and self.P == other.P - and self.binning_x == other.binning_x - and self.binning_y == other.binning_y - and self.frame_id == other.frame_id - ) - - def to_rerun( - self, - image_plane_distance: float = 1.0, - # These are defaults for a typical RGB camera with a known transform - # - # TODO this should be done by the actual emitting modules, - # they know the camera image topic, spatial relationships etc - # - # Poor CameraInfo class has no idea on this - # We just provide the parameters here for convenience in case your - # module doesn't implement this correctly - image_topic: str | None = None, - optical_frame: str | None = None, - ) -> RerunData: - """Convert to Rerun Pinhole archetype for camera frustum visualization. - - Args: - image_plane_distance: Distance to draw the image plane in the frustum - - Returns: - rr.Pinhole archetype for logging to Rerun - """ - import rerun as rr - - # Extract intrinsics from K matrix - # K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] - fx, fy = self.K[0], self.K[4] - cx, cy = self.K[2], self.K[5] - - pinhole_kwargs: dict[str, Any] = dict( - focal_length=[fx, fy], - principal_point=[cx, cy], - width=self.width, - height=self.height, - image_plane_distance=image_plane_distance, - ) - - # If no image topic is specified, We don't know which Image this CameraInfo refers to - # return just the pinhole - if not image_topic: - return rr.Pinhole(**pinhole_kwargs) - - return [(image_topic, self.to_rerun_pinhole(image_plane_distance, optical_frame))] - - def to_rerun_pinhole( - self, image_plane_distance: float = 1.0, optical_frame: str | None = None - ) -> Any: - """The Pinhole to log on this camera's image entity, parented to its optical frame.""" - import rerun as rr - - fx, fy = self.K[0], self.K[4] - cx, cy = self.K[2], self.K[5] - # Re-parent the image entity here rather than with a separate - # Transform3D, which would give it a second parent and lose the frustum. - frame = optical_frame or self.frame_id - return rr.Pinhole( - focal_length=[fx, fy], - principal_point=[cx, cy], - width=self.width, - height=self.height, - image_plane_distance=image_plane_distance, - parent_frame=f"tf#/{frame}" if frame else None, - ) - - -class CalibrationProvider: - """Provides lazy-loaded access to camera calibration YAML files in a directory.""" - - def __init__(self, calibration_dir) -> None: # type: ignore[no-untyped-def] - """Initialize with a directory containing calibration YAML files. - - Args: - calibration_dir: Path to directory containing .yaml calibration files - """ - from pathlib import Path - - self._calibration_dir = Path(calibration_dir) - self._cache = {} # type: ignore[var-annotated] - - def _to_snake_case(self, name: str) -> str: - """Convert PascalCase to snake_case.""" - import re - - # Insert underscore before capital letters (except first char) - s1 = re.sub("(.)([A-Z][a-z]+)", r"\1_\2", name) - # Insert underscore before capital letter followed by lowercase - return re.sub("([a-z0-9])([A-Z])", r"\1_\2", s1).lower() - - def _find_yaml_file(self, name: str): # type: ignore[no-untyped-def] - """Find YAML file matching the given name (tries both snake_case and exact match). - - Args: - name: Attribute name to look for - - Returns: - Path to YAML file if found, None otherwise - """ - # Try exact match first - yaml_file = self._calibration_dir / f"{name}.yaml" - if yaml_file.exists(): - return yaml_file - - # Try snake_case conversion for PascalCase names - snake_name = self._to_snake_case(name) - if snake_name != name: - yaml_file = self._calibration_dir / f"{snake_name}.yaml" - if yaml_file.exists(): - return yaml_file - - return None - - def __getattr__(self, name: str) -> CameraInfo: - """Load calibration YAML file on first access. - - Supports both snake_case and PascalCase attribute names. - For example, both 'single_webcam' and 'SingleWebcam' will load 'single_webcam.yaml'. - - Args: - name: Attribute name (can be PascalCase or snake_case) - - Returns: - CameraInfo object loaded from the YAML file - - Raises: - AttributeError: If no matching YAML file exists - """ - # Check cache first - if name in self._cache: - return self._cache[name] # type: ignore[no-any-return] - - # Also check if the snake_case version is cached (for PascalCase access) - snake_name = self._to_snake_case(name) - if snake_name != name and snake_name in self._cache: - return self._cache[snake_name] # type: ignore[no-any-return] - - # Find matching YAML file - yaml_file = self._find_yaml_file(name) - if not yaml_file: - raise AttributeError(f"No calibration file found for: {name}") - - # Load and cache the CameraInfo - camera_info = CameraInfo.from_yaml(str(yaml_file)) - - # Cache both the requested name and the snake_case version - self._cache[name] = camera_info - if snake_name != name: - self._cache[snake_name] = camera_info - - return camera_info - - def __dir__(self): # type: ignore[no-untyped-def] - """List available calibrations in both snake_case and PascalCase.""" - calibrations = [] - if self._calibration_dir.exists() and self._calibration_dir.is_dir(): - yaml_files = self._calibration_dir.glob("*.yaml") - for f in yaml_files: - stem = f.stem - calibrations.append(stem) - # Add PascalCase version - pascal = "".join(word.capitalize() for word in stem.split("_")) - if pascal != stem: - calibrations.append(pascal) - return calibrations diff --git a/dimos/msgs/sensor_msgs/CompressedImage.py b/dimos/msgs/sensor_msgs/CompressedImage.py deleted file mode 100644 index f1853986fc5..00000000000 --- a/dimos/msgs/sensor_msgs/CompressedImage.py +++ /dev/null @@ -1,175 +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 dataclasses import dataclass, field -import time -from typing import TYPE_CHECKING, Any, Literal - -from dimos_lcm.sensor_msgs.CompressedImage import CompressedImage as LCMCompressedImage -from dimos_lcm.std_msgs.Header import Header - -from dimos.types.timestamped import Timestamped, to_human_readable - -# cv2/turbojpeg/Image are imported lazily so this module stays cheap for -# byte-level consumers that never encode or decode pixels. -if TYPE_CHECKING: - from dimos.msgs.sensor_msgs.Image import Image - -CompressionFormat = Literal["jpeg", "png", "jxl"] - - -@dataclass -class CompressedImage(Timestamped): - """Compressed image bytes (JPEG/PNG/JPEG-XL) — ROS sensor_msgs/CompressedImage.""" - - msg_name = "sensor_msgs.CompressedImage" - - data: bytes = b"" - format: str = "jpeg" - frame_id: str = "" - ts: float = field(default_factory=time.time) - - def __str__(self) -> str: - return ( - f"CompressedImage(format={self.format}, bytes={len(self.data)}, " - f"ts={to_human_readable(self.ts)})" - ) - - @classmethod - def from_image( - cls, - image: Image, - format: CompressionFormat = "jpeg", - quality: int = 75, - max_width: int | None = None, - effort: int | None = None, - ) -> CompressedImage: - """Encode a raw Image. - - JPEG rejects 16-bit/depth formats; PNG rejects float depth. JXL takes - everything: lossless for 16-bit/float data (depth must survive the wire - exactly), `quality` for uint8. `effort` (jxl only, 1-9) trades encode - cpu for size; defaults are pinned for camera-rate streaming. - """ - from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - - if not isinstance(image, Image): - raise TypeError(f"from_image expects Image, got {type(image).__name__}") - if max_width is not None: - image, _ = image.resize_to_fit(max_width, max_width) - if format == "jpeg": - if image.format in (ImageFormat.GRAY16, ImageFormat.DEPTH, ImageFormat.DEPTH16): - raise ValueError(f"JPEG cannot encode {image.format.value}; use format='png'") - data = image.to_jpeg_bytes(quality=quality) - elif format == "png": - import cv2 - - if image.format in (ImageFormat.DEPTH, ImageFormat.DEPTH16): - raise ValueError(f"PNG cannot encode {image.format.value}") - arr = image.data if image.channels == 1 else image.to_bgr().data - ok, buf = cv2.imencode(".png", arr) - if not ok: - raise ValueError("PNG encoding failed") - data = buf.tobytes() - elif format == "jxl": - import imagecodecs - import numpy as np - - # zero-copy passthrough unless channel order actually needs fixing - if image.channels == 1 or image.format is ImageFormat.RGB: - arr = image.data - else: - arr = image.to_rgb().data - if arr.dtype not in (np.uint8, np.uint16, np.float32): - raise ValueError(f"JXL cannot encode dtype {arr.dtype}") - arr = np.ascontiguousarray(arr) - # libjxl's default effort=7 costs 83-300ms per 720p frame; these - # pins keep camera-rate (13ms lossy / 3ms lossless) for ~5% size - if arr.dtype == np.uint8: - data = bytes(imagecodecs.jpegxl_encode(arr, level=quality, effort=effort or 3)) - else: - data = bytes(imagecodecs.jpegxl_encode(arr, lossless=True, effort=effort or 1)) - else: - raise ValueError(f"unsupported format {format!r}") - return cls(data=data, format=format, frame_id=image.frame_id, ts=image.ts) - - def decode(self) -> Image: - """Decompress to a raw Image; ts/frame_id preserved.""" - from dimos.msgs.sensor_msgs.Image import Image, ImageFormat - - if self.format.startswith("jpeg"): - from turbojpeg import TJPF_RGB, TurboJPEG - - arr = TurboJPEG().decode(self.data, pixel_format=TJPF_RGB) - fmt = ImageFormat.RGB - elif self.format.startswith("png"): - import cv2 - import numpy as np - - arr = cv2.imdecode(np.frombuffer(self.data, dtype=np.uint8), cv2.IMREAD_UNCHANGED) - if arr is None: - raise ValueError("PNG decoding failed") - if arr.ndim == 2: - fmt = ImageFormat.GRAY16 if arr.dtype == np.uint16 else ImageFormat.GRAY - elif arr.shape[2] == 4: - fmt = ImageFormat.BGRA - else: - fmt = ImageFormat.BGR - elif self.format.startswith("jxl"): - import imagecodecs - import numpy as np - - arr = imagecodecs.jpegxl_decode(self.data) - if arr.ndim == 2: - fmt = {np.float32: ImageFormat.DEPTH, np.uint16: ImageFormat.GRAY16}.get( - arr.dtype.type, ImageFormat.GRAY - ) - else: - fmt = ImageFormat.RGBA if arr.shape[2] == 4 else ImageFormat.RGB - else: - raise ValueError(f"unsupported format {self.format!r}") - return Image(data=arr, format=fmt, frame_id=self.frame_id, ts=self.ts) - - def lcm_encode(self, frame_id: str | None = None) -> bytes: - msg = LCMCompressedImage() - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - msg.format = self.format - msg.data = self.data - msg.data_length = len(self.data) - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: Any) -> CompressedImage: - msg = LCMCompressedImage.lcm_decode(data) - return cls( - data=bytes(msg.data), - format=msg.format, - frame_id=msg.header.frame_id, - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9, - ) - - def to_rerun(self) -> Any: - import rerun as rr - - if self.format.startswith(("jpeg", "png")): - media_type = "image/jpeg" if self.format.startswith("jpeg") else "image/png" - return rr.EncodedImage(contents=self.data, media_type=media_type) - # formats rerun's EncodedImage can't decode: send pixels instead - return self.decode().to_rerun() diff --git a/dimos/msgs/sensor_msgs/Image.py b/dimos/msgs/sensor_msgs/Image.py deleted file mode 100644 index 05b05225757..00000000000 --- a/dimos/msgs/sensor_msgs/Image.py +++ /dev/null @@ -1,700 +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 base64 -from dataclasses import dataclass, field -from enum import Enum -import time -from typing import TYPE_CHECKING, Any, Literal, TypedDict -import warnings - -from dimos_lcm.sensor_msgs.Image import Image as LCMImage -from dimos_lcm.std_msgs.Header import Header -import numpy as np -import reactivex as rx -from reactivex import operators as ops -from turbojpeg import TJPF_RGB - -from dimos.types.timestamped import Timestamped, TimestampedBufferCollection, to_human_readable -from dimos.utils.reactive import quality_barrier -from dimos.utils.turbojpeg import get_turbojpeg - -if TYPE_CHECKING: - from collections.abc import Callable - import os - - from reactivex.abc import SchedulerBase - from reactivex.observable import Observable - - -class ImageFormat(Enum): - BGR = "BGR" - RGB = "RGB" - RGBA = "RGBA" - BGRA = "BGRA" - GRAY = "GRAY" - GRAY16 = "GRAY16" - DEPTH = "DEPTH" - DEPTH16 = "DEPTH16" - - -class AgentImageMessage(TypedDict): - """Type definition for agent-compatible image representation.""" - - type: Literal["image"] - source_type: Literal["base64"] - mime_type: Literal["image/jpeg", "image/png"] - data: str # Base64 encoded image data - - -@dataclass -class Image(Timestamped): - """Simple NumPy-based image container.""" - - msg_name = "sensor_msgs.Image" - - data: np.ndarray[Any, np.dtype[Any]] = field( - default_factory=lambda: np.zeros((1, 1, 3), dtype=np.uint8) - ) - format: ImageFormat = field(default=ImageFormat.BGR) - frame_id: str = field(default="") - ts: float = field(default_factory=time.time) - - def __post_init__(self) -> None: - if not isinstance(self.data, np.ndarray): - self.data = np.asarray(self.data) - if self.data.ndim < 2: - raise ValueError("Image requires a 2D/3D NumPy array") - - def __str__(self) -> str: - return ( - f"Image(shape={self.shape}, format={self.format.value}, dtype={self.dtype}, " - f"ts={to_human_readable(self.ts)})" - ) - - def __repr__(self) -> str: - return f"Image(shape={self.shape}, format={self.format.value}, dtype={self.dtype}, frame_id='{self.frame_id}', ts={self.ts})" - - def __eq__(self, other: object) -> bool: - if not isinstance(other, Image): - return False - return ( - np.array_equal(self.data, other.data) - and self.format == other.format - and self.frame_id == other.frame_id - and abs(self.ts - other.ts) < 1e-6 - ) - - def __len__(self) -> int: - return int(self.height * self.width) - - def __getstate__(self) -> dict[str, Any]: - return {"data": self.data, "format": self.format, "frame_id": self.frame_id, "ts": self.ts} - - def __setstate__(self, state: dict[str, Any]) -> None: - self.data = state.get("data", np.zeros((1, 1, 3), dtype=np.uint8)) - self.format = state.get("format", ImageFormat.BGR) - self.frame_id = state.get("frame_id", "") - self.ts = state.get("ts", time.time()) - - @property - def height(self) -> int: - return int(self.data.shape[0]) - - @property - def width(self) -> int: - return int(self.data.shape[1]) - - @property - def channels(self) -> int: - if self.data.ndim == 2: - return 1 - if self.data.ndim == 3: - return int(self.data.shape[2]) - raise ValueError("Invalid image dimensions") - - @property - def shape(self) -> tuple[int, ...]: - return tuple(self.data.shape) - - @property - def dtype(self) -> np.dtype[Any]: - return self.data.dtype - - def copy(self) -> Image: - return Image(data=self.data.copy(), format=self.format, frame_id=self.frame_id, ts=self.ts) - - @classmethod - def from_numpy( - cls, - np_image: np.ndarray, - format: ImageFormat = ImageFormat.BGR, - frame_id: str = "", - ts: float | None = None, - ) -> Image: - return cls( - data=np.asarray(np_image), - format=format, - frame_id=frame_id, - ts=ts if ts is not None else time.time(), - ) - - @classmethod - def from_file( - cls, - filepath: str | os.PathLike[str], - format: ImageFormat = ImageFormat.RGB, - ) -> Image: - import cv2 - - arr = cv2.imread(str(filepath), cv2.IMREAD_UNCHANGED) - if arr is None: - raise ValueError(f"Could not load image from {filepath}") - if arr.ndim == 2: - detected = ImageFormat.GRAY16 if arr.dtype == np.uint16 else ImageFormat.GRAY - elif arr.shape[2] == 3: - detected = ImageFormat.BGR # OpenCV default - elif arr.shape[2] == 4: - detected = ImageFormat.BGRA # OpenCV default - else: - detected = format - return cls(data=arr, format=detected) - - @classmethod - def from_opencv( - cls, - cv_image: np.ndarray, - format: ImageFormat = ImageFormat.BGR, - frame_id: str = "", - ts: float | None = None, - ) -> Image: - """Construct from an OpenCV image (NumPy array).""" - return cls( - data=cv_image, - format=format, - frame_id=frame_id, - ts=ts if ts is not None else time.time(), - ) - - def to_opencv(self) -> np.ndarray: - """Convert to OpenCV BGR format.""" - import cv2 - - arr = self.data - if self.format == ImageFormat.BGR: - return arr - if self.format == ImageFormat.RGB: - return cv2.cvtColor(arr, cv2.COLOR_RGB2BGR) - if self.format == ImageFormat.RGBA: - return cv2.cvtColor(arr, cv2.COLOR_RGBA2BGR) - if self.format == ImageFormat.BGRA: - return cv2.cvtColor(arr, cv2.COLOR_BGRA2BGR) - if self.format in ( - ImageFormat.GRAY, - ImageFormat.GRAY16, - ImageFormat.DEPTH, - ImageFormat.DEPTH16, - ): - return arr - raise ValueError(f"Unsupported format: {self.format}") - - def as_numpy(self) -> np.ndarray: - """Get image data as numpy array.""" - return self.data - - def to_rgb(self) -> Image: - import cv2 - - if self.format == ImageFormat.RGB: - return self.copy() - arr = self.data - if self.format == ImageFormat.BGR: - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGR2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGBA: - warnings.warn("to_rgb() drops alpha channel from RGBA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGBA2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.BGRA: - warnings.warn("to_rgb() drops alpha channel from BGRA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGRA2RGB), - format=ImageFormat.RGB, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH16): - gray8 = (arr / 256).astype(np.uint8) if self.format != ImageFormat.GRAY else arr - rgb = cv2.cvtColor(gray8, cv2.COLOR_GRAY2RGB) - return Image(data=rgb, format=ImageFormat.RGB, frame_id=self.frame_id, ts=self.ts) - return self.copy() - - def to_bgr(self) -> Image: - import cv2 - - if self.format == ImageFormat.BGR: - return self.copy() - arr = self.data - if self.format == ImageFormat.RGB: - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGB2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGBA: - warnings.warn("to_bgr() drops alpha channel from RGBA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_RGBA2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.BGRA: - warnings.warn("to_bgr() drops alpha channel from BGRA image", stacklevel=2) - return Image( - data=cv2.cvtColor(arr, cv2.COLOR_BGRA2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH16): - gray8 = (arr / 256).astype(np.uint8) if self.format != ImageFormat.GRAY else arr - return Image( - data=cv2.cvtColor(gray8, cv2.COLOR_GRAY2BGR), - format=ImageFormat.BGR, - frame_id=self.frame_id, - ts=self.ts, - ) - return self.copy() - - def to_grayscale(self) -> Image: - import cv2 - - if self.format in (ImageFormat.GRAY, ImageFormat.GRAY16, ImageFormat.DEPTH): - return self.copy() - if self.format == ImageFormat.BGR: - return Image( - data=cv2.cvtColor(self.data, cv2.COLOR_BGR2GRAY), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format == ImageFormat.RGB: - return Image( - data=cv2.cvtColor(self.data, cv2.COLOR_RGB2GRAY), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - if self.format in (ImageFormat.RGBA, ImageFormat.BGRA): - code = cv2.COLOR_RGBA2GRAY if self.format == ImageFormat.RGBA else cv2.COLOR_BGRA2GRAY - return Image( - data=cv2.cvtColor(self.data, code), - format=ImageFormat.GRAY, - frame_id=self.frame_id, - ts=self.ts, - ) - raise ValueError(f"Unsupported format: {self.format}") - - def to_rerun(self) -> Any: - """Convert to a Rerun archetype: JPEG-encoded for color images, raw for depth.""" - import rerun as rr - - match self.format: - case ImageFormat.DEPTH | ImageFormat.DEPTH16: - return rr.DepthImage(self.data) - case ImageFormat.GRAY16: - return rr.Image(self.data, color_model="L") - case _: - return rr.EncodedImage(contents=self.to_jpeg_bytes(), media_type="image/jpeg") - - def resize(self, width: int, height: int, interpolation: int | None = None) -> Image: - import cv2 - - if interpolation is None: - interpolation = cv2.INTER_LINEAR - return Image( - data=cv2.resize(self.data, (width, height), interpolation=interpolation), - format=self.format, - frame_id=self.frame_id, - ts=self.ts, - ) - - def resize_to_fit( - self, max_width: int, max_height: int, interpolation: int | None = None - ) -> tuple[Image, float]: - """Resize image to fit within max dimensions while preserving aspect ratio. - - Only scales down if image exceeds max dimensions. Returns self if already fits. - - Returns: - Tuple of (resized_image, scale_factor). Scale factor is 1.0 if no resize needed. - """ - if self.width <= max_width and self.height <= max_height: - return self, 1.0 - - scale = min(max_width / self.width, max_height / self.height) - new_width = int(self.width * scale) - new_height = int(self.height * scale) - return self.resize(new_width, new_height, interpolation), scale - - def crop(self, x: int, y: int, width: int, height: int) -> Image: - """Crop the image to the specified region. - - Args: - x: Starting x coordinate (left edge) - y: Starting y coordinate (top edge) - width: Width of the cropped region - height: Height of the cropped region - - Returns: - A new Image containing the cropped region - """ - img_height, img_width = self.data.shape[:2] - - # Clamp the crop region to image bounds - x = max(0, min(x, img_width)) - y = max(0, min(y, img_height)) - x_end = min(x + width, img_width) - y_end = min(y + height, img_height) - - # Perform the crop using array slicing - if self.data.ndim == 2: - cropped_data = self.data[y:y_end, x:x_end] - else: - cropped_data = self.data[y:y_end, x:x_end, :] - - return Image(data=cropped_data, format=self.format, frame_id=self.frame_id, ts=self.ts) - - @property - def brightness(self) -> float: - """Return mean brightness in [0, 1]. - - Strides to ~256px on the long edge first — ~O(N/step²) cheaper than - reading every pixel, and the mean converges quickly (CLT). - """ - max_val = 65535.0 if self.format in (ImageFormat.GRAY16, ImageFormat.DEPTH16) else 255.0 - step = max(1, max(self.data.shape[:2]) // 256) - return float(self.data[::step, ::step].mean() / max_val) - - @property - def sharpness(self) -> float: - """Return sharpness score. - - Downsamples to ~160px wide before computing Laplacian variance - for fast evaluation (~10-20x cheaper than full-res Sobel). - """ - import cv2 - - gray = self.to_grayscale().data - # Downsample to ~160px wide for cheap evaluation - h, w = gray.shape[:2] - if w > 160: - scale = 160.0 / w - gray = cv2.resize(gray, (160, int(h * scale)), interpolation=cv2.INTER_AREA) - lap_var = cv2.Laplacian(gray, cv2.CV_32F).var() - if lap_var <= 0: - return 0.0 - return float(np.clip((np.log10(lap_var + 1) - 1.0) / 3.0, 0.0, 1.0)) - - def save(self, filepath: str) -> bool: - import cv2 - - arr = self.to_opencv() - return cv2.imwrite(filepath, arr) - - def to_base64( - self, - quality: int = 80, - *, - max_width: int | None = None, - max_height: int | None = None, - ) -> str: - """Encode the image as a base64 JPEG string. - - Args: - quality: JPEG quality (0-100). - max_width: Optional maximum width to constrain the encoded image. - max_height: Optional maximum height to constrain the encoded image. - - Returns: - Base64-encoded JPEG representation of the image. - """ - import cv2 - - bgr_image = self.to_bgr().to_opencv() - height, width = bgr_image.shape[:2] - - scale = 1.0 - if max_width is not None and width > max_width: - scale = min(scale, max_width / width) - if max_height is not None and height > max_height: - scale = min(scale, max_height / height) - - if scale < 1.0: - new_width = max(1, round(width * scale)) - new_height = max(1, round(height * scale)) - bgr_image = cv2.resize(bgr_image, (new_width, new_height), interpolation=cv2.INTER_AREA) - - encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), int(np.clip(quality, 0, 100))] - success, buffer = cv2.imencode(".jpg", bgr_image, encode_param) - if not success: - raise ValueError("Failed to encode image as JPEG") - - return base64.b64encode(buffer.tobytes()).decode("utf-8") - - def agent_encode(self) -> AgentImageMessage: - return [ # type: ignore[return-value] - { - "type": "image_url", - "image_url": {"url": f"data:image/jpeg;base64,{self.to_base64()}"}, - } - ] - - # LCM encode/decode - def lcm_encode(self, frame_id: str | None = None) -> bytes: - """Convert to LCM Image message.""" - msg = LCMImage() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - # Set timestamp - if self.ts is not None: - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - else: - now = time.time() - msg.header.stamp.sec = int(now) - msg.header.stamp.nsec = int((now - int(now)) * 1e9) - - # Image properties - msg.height = self.height - msg.width = self.width - msg.encoding = _get_lcm_encoding(self.format, self.dtype) - msg.is_bigendian = False - - # Calculate step (bytes per row) - channels = 1 if self.data.ndim == 2 else self.data.shape[2] - msg.step = self.width * self.dtype.itemsize * channels - - view = memoryview(np.ascontiguousarray(self.data)).cast("B") # type: ignore[arg-type] - msg.data_length = len(view) - msg.data = view - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: Any) -> Image: - msg = LCMImage.lcm_decode(data) - - # JPEG-compressed images use a different decode path. - if msg.encoding == "jpeg": - return cls.lcm_jpeg_decode(data, **kwargs) - - fmt, dtype, channels = _parse_lcm_encoding(msg.encoding) - arr: np.ndarray[Any, Any] = np.frombuffer(msg.data, dtype=dtype) - if channels == 1: - arr = arr.reshape((msg.height, msg.width)) - else: - arr = arr.reshape((msg.height, msg.width, channels)) - return cls( - data=arr, - format=fmt, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=( - msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") - and hasattr(msg.header, "stamp") - and msg.header.stamp.sec > 0 - else time.time() - ), - ) - - def to_jpeg_bytes(self, quality: int = 75) -> bytes: - """Encode this image as JPEG bytes using TurboJPEG. - - Args: - quality: JPEG compression quality (0-100, default 75). - - Returns: - Raw JPEG bytes. - """ - # Canonicalize to RGB so JPEG bytes are deterministic regardless of input format. - rgb_array = self.to_rgb().data - return get_turbojpeg().encode(rgb_array, quality=quality, pixel_format=TJPF_RGB) # type: ignore[no-any-return] - - def lcm_jpeg_encode(self, quality: int = 75, frame_id: str | None = None) -> bytes: - """Convert to LCM Image message with JPEG-compressed data. - - Args: - quality: JPEG compression quality (0-100, default 75) - frame_id: Optional frame ID override - - Returns: - LCM-encoded bytes with JPEG-compressed image data - """ - msg = LCMImage() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - # Set timestamp - if self.ts is not None: - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - else: - now = time.time() - msg.header.stamp.sec = int(now) - msg.header.stamp.nsec = int((now - int(now)) * 1e9) - - jpeg_data = self.to_jpeg_bytes(quality=quality) - - # Store JPEG data and metadata - msg.height = self.height - msg.width = self.width - msg.encoding = "jpeg" - msg.is_bigendian = False - msg.step = 0 # Not applicable for compressed format - - msg.data_length = len(jpeg_data) - msg.data = jpeg_data - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_jpeg_decode(cls, data: bytes, **kwargs: Any) -> Image: - """Decode an LCM Image message with JPEG-compressed data. - - Args: - data: LCM-encoded bytes containing JPEG-compressed image - - Returns: - Image instance - """ - msg = LCMImage.lcm_decode(data) - - if msg.encoding != "jpeg": - raise ValueError(f"Expected JPEG encoding, got {msg.encoding}") - - rgb_array = get_turbojpeg().decode(msg.data, pixel_format=TJPF_RGB) - - return cls( - data=rgb_array, - format=ImageFormat.RGB, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=( - msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") - and hasattr(msg.header, "stamp") - and msg.header.stamp.sec > 0 - else time.time() - ), - ) - - -def sharpness_window(target_frequency: float, source: Observable[Image]) -> Observable[Image]: - """Emit the sharpest Image seen within each sliding time window.""" - from reactivex.scheduler import ThreadPoolScheduler - - if target_frequency <= 0: - raise ValueError("target_frequency must be positive") - - window = TimestampedBufferCollection(1.0 / target_frequency) # type: ignore[var-annotated] - source.subscribe(window.add) - - thread_scheduler = ThreadPoolScheduler(max_workers=1) - - def find_best(*_args: Any) -> Image | None: - if len(window) == 0: - return None - return max(window, key=lambda img: img.sharpness) # type: ignore[no-any-return] - - return rx.interval(1.0 / target_frequency).pipe( # type: ignore[misc] - ops.observe_on(thread_scheduler), - ops.map(find_best), - ops.filter(lambda img: img is not None), - ) - - -def sharpness_barrier( - target_frequency: float, scheduler: SchedulerBase | None = None -) -> Callable[[Observable[Image]], Observable[Image]]: - """Select the sharpest Image within each time window.""" - if target_frequency <= 0: - raise ValueError("target_frequency must be positive") - return quality_barrier(lambda image: image.sharpness, target_frequency, scheduler) - - -def _get_lcm_encoding(fmt: ImageFormat, dtype: np.dtype) -> str: - if fmt == ImageFormat.GRAY: - if dtype == np.uint8: - return "mono8" - if dtype == np.uint16: - return "mono16" - if fmt == ImageFormat.GRAY16: - return "mono16" - if fmt == ImageFormat.RGB: - return "rgb8" - if fmt == ImageFormat.RGBA: - return "rgba8" - if fmt == ImageFormat.BGR: - return "bgr8" - if fmt == ImageFormat.BGRA: - return "bgra8" - if fmt == ImageFormat.DEPTH: - if dtype == np.float32: - return "32FC1" - if dtype == np.float64: - return "64FC1" - if fmt == ImageFormat.DEPTH16: - if dtype == np.uint16: - return "16UC1" - if dtype == np.int16: - return "16SC1" - raise ValueError(f"Unsupported LCM encoding for fmt={fmt}, dtype={dtype}") - - -def _parse_lcm_encoding(enc: str) -> tuple[ImageFormat, type, int]: - m = { - "mono8": (ImageFormat.GRAY, np.uint8, 1), - "mono16": (ImageFormat.GRAY16, np.uint16, 1), - "rgb8": (ImageFormat.RGB, np.uint8, 3), - "rgba8": (ImageFormat.RGBA, np.uint8, 4), - "bgr8": (ImageFormat.BGR, np.uint8, 3), - "bgra8": (ImageFormat.BGRA, np.uint8, 4), - "32FC1": (ImageFormat.DEPTH, np.float32, 1), - "32FC3": (ImageFormat.RGB, np.float32, 3), - "64FC1": (ImageFormat.DEPTH, np.float64, 1), - "16UC1": (ImageFormat.DEPTH16, np.uint16, 1), - "16SC1": (ImageFormat.DEPTH16, np.int16, 1), - } - if enc not in m: - raise ValueError(f"Unsupported encoding: {enc}") - return m[enc] diff --git a/dimos/msgs/sensor_msgs/Imu.py b/dimos/msgs/sensor_msgs/Imu.py deleted file mode 100644 index e1cd1f6b621..00000000000 --- a/dimos/msgs/sensor_msgs/Imu.py +++ /dev/null @@ -1,125 +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 time - -from dimos_lcm.sensor_msgs.Imu import Imu as LCMImu - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.types.timestamped import Timestamped - - -class Imu(Timestamped): - """IMU sensor message mirroring ROS sensor_msgs/Imu. - - Contains orientation, angular velocity, and linear acceleration - with optional covariance matrices (3x3 row-major as flat 9-element lists). - """ - - msg_name = "sensor_msgs.Imu" - - def __init__( - self, - angular_velocity: Vector3 | None = None, - linear_acceleration: Vector3 | None = None, - orientation: Quaternion | None = None, - orientation_covariance: list[float] | None = None, - angular_velocity_covariance: list[float] | None = None, - linear_acceleration_covariance: list[float] | None = None, - frame_id: str = "imu_link", - ts: float | None = None, - ) -> None: - self.ts = ts if ts is not None else time.time() - self.frame_id = frame_id - self.angular_velocity = angular_velocity or Vector3(0.0, 0.0, 0.0) - self.linear_acceleration = linear_acceleration or Vector3(0.0, 0.0, 0.0) - self.orientation = orientation or Quaternion(0.0, 0.0, 0.0, 1.0) - self.orientation_covariance = orientation_covariance or [0.0] * 9 - self.angular_velocity_covariance = angular_velocity_covariance or [0.0] * 9 - self.linear_acceleration_covariance = linear_acceleration_covariance or [0.0] * 9 - - def lcm_encode(self) -> bytes: - msg = LCMImu() - [msg.header.stamp.sec, msg.header.stamp.nsec] = self.ros_timestamp() - msg.header.frame_id = self.frame_id - - msg.orientation.x = self.orientation.x - msg.orientation.y = self.orientation.y - msg.orientation.z = self.orientation.z - msg.orientation.w = self.orientation.w - msg.orientation_covariance = self.orientation_covariance - - msg.angular_velocity.x = self.angular_velocity.x - msg.angular_velocity.y = self.angular_velocity.y - msg.angular_velocity.z = self.angular_velocity.z - msg.angular_velocity_covariance = self.angular_velocity_covariance - - msg.linear_acceleration.x = self.linear_acceleration.x - msg.linear_acceleration.y = self.linear_acceleration.y - msg.linear_acceleration.z = self.linear_acceleration.z - msg.linear_acceleration_covariance = self.linear_acceleration_covariance - - return msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Imu: - msg = LCMImu.lcm_decode(data) - ts = msg.header.stamp.sec + (msg.header.stamp.nsec / 1_000_000_000) - return cls( - angular_velocity=Vector3( - msg.angular_velocity.x, - msg.angular_velocity.y, - msg.angular_velocity.z, - ), - linear_acceleration=Vector3( - msg.linear_acceleration.x, - msg.linear_acceleration.y, - msg.linear_acceleration.z, - ), - orientation=Quaternion( - msg.orientation.x, - msg.orientation.y, - msg.orientation.z, - msg.orientation.w, - ), - orientation_covariance=list(msg.orientation_covariance), - angular_velocity_covariance=list(msg.angular_velocity_covariance), - linear_acceleration_covariance=list(msg.linear_acceleration_covariance), - frame_id=msg.header.frame_id, - ts=ts, - ) - - def __str__(self) -> str: - return ( - f"Imu(frame_id='{self.frame_id}', " - f"gyro=({self.angular_velocity.x:.3f}, {self.angular_velocity.y:.3f}, {self.angular_velocity.z:.3f}), " - f"accel=({self.linear_acceleration.x:.3f}, {self.linear_acceleration.y:.3f}, {self.linear_acceleration.z:.3f}))" - ) - - def __repr__(self) -> str: - return ( - f"Imu(ts={self.ts}, frame_id='{self.frame_id}', " - f"angular_velocity={self.angular_velocity}, " - f"linear_acceleration={self.linear_acceleration}, " - f"orientation={self.orientation})" - ) - - def to_rerun(self, axis_length: float = 0.2): # type: ignore[no-untyped-def] - """Orientation axes for rerun.""" - import rerun as rr - - return rr.TransformAxes3D(axis_length=axis_length) diff --git a/dimos/msgs/sensor_msgs/ImuInfo.py b/dimos/msgs/sensor_msgs/ImuInfo.py deleted file mode 100644 index 216d7e42a58..00000000000 --- a/dimos/msgs/sensor_msgs/ImuInfo.py +++ /dev/null @@ -1,158 +0,0 @@ -# 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. - -"""IMU noise-model information, the inertial sibling of ``CameraInfo``. - -Where the IMU sits comes from tf (``header.frame_id`` names the frame), just as -camera extrinsics do; this message carries what tf cannot: the continuous-time -noise model consumers such as VIO preintegration need. The values are -Allan-variance constants, measured once per model (or per unit); IMUs do not -report them at runtime, so a driver publishes them from its config the same way -``camera_info`` is published alongside images. - -LCM schema (fingerprint-compatible bindings must be generated from this):: - - package sensor_msgs; - - struct ImuInfo { - std_msgs.Header header; - double gyro_noise_density; - double gyro_random_walk; - double accel_noise_density; - double accel_random_walk; - double frequency; - } -""" - -from __future__ import annotations - -from io import BytesIO -import struct - -from dimos_lcm.std_msgs.Header import Header - -from dimos.types.timestamped import Timestamped - -# lcm-gen's base hash for the struct above; the wire fingerprint mixes in -# Header's recursively, matching the generated C++ in dimSLAM. -_BASE_HASH = 0x437A196CBD6B4E49 - - -def _packed_fingerprint() -> bytes: - tmphash = (_BASE_HASH + Header._get_hash_recursive([ImuInfo])) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return struct.pack(">Q", tmphash) - - -class ImuInfo(Timestamped): - """IMU noise model: Allan-variance constants plus the delivered sample rate.""" - - msg_name = "sensor_msgs.ImuInfo" - - def __init__( - self, - gyro_noise_density: float = 0.0, - gyro_random_walk: float = 0.0, - accel_noise_density: float = 0.0, - accel_random_walk: float = 0.0, - frequency: float = 0.0, - frame_id: str = "", - ts: float | None = None, - ) -> None: - """Initialize ImuInfo. - - Args: - gyro_noise_density: Gyroscope white noise, rad/s/sqrt(Hz) - gyro_random_walk: Gyroscope bias random walk, rad/s^2/sqrt(Hz) - accel_noise_density: Accelerometer white noise, m/s^2/sqrt(Hz) - accel_random_walk: Accelerometer bias random walk, m/s^3/sqrt(Hz) - frequency: Rate the samples are actually delivered at, Hz - frame_id: The IMU frame; tf places it against the rest of the rig - ts: Timestamp (defaults to now) - """ - import time - - super().__init__(ts if ts is not None else time.time()) - self.gyro_noise_density = gyro_noise_density - self.gyro_random_walk = gyro_random_walk - self.accel_noise_density = accel_noise_density - self.accel_random_walk = accel_random_walk - self.frequency = frequency - self.frame_id = frame_id - - def with_ts(self, ts: float) -> ImuInfo: - """Return a copy of this ImuInfo with the given timestamp.""" - return ImuInfo( - gyro_noise_density=self.gyro_noise_density, - gyro_random_walk=self.gyro_random_walk, - accel_noise_density=self.accel_noise_density, - accel_random_walk=self.accel_random_walk, - frequency=self.frequency, - frame_id=self.frame_id, - ts=ts, - ) - - def with_frame_id(self, frame_id: str) -> ImuInfo: - """Return a copy of this ImuInfo stamped with the given frame.""" - copy = self.with_ts(self.ts) - copy.frame_id = frame_id - return copy - - def lcm_encode(self) -> bytes: - header = Header() - header.seq = 0 - header.frame_id = self.frame_id - header.stamp.sec = int(self.ts) - header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - buf = BytesIO() - buf.write(_packed_fingerprint()) - header._encode_one(buf) - buf.write( - struct.pack( - ">ddddd", - self.gyro_noise_density, - self.gyro_random_walk, - self.accel_noise_density, - self.accel_random_walk, - self.frequency, - ) - ) - return buf.getvalue() - - @classmethod - def lcm_decode(cls, data: bytes) -> ImuInfo: - buf = BytesIO(data) - if buf.read(8) != _packed_fingerprint(): - raise ValueError("Decode error") - header = Header._decode_one(buf) - values = struct.unpack(">ddddd", buf.read(40)) - return cls( - gyro_noise_density=values[0], - gyro_random_walk=values[1], - accel_noise_density=values[2], - accel_random_walk=values[3], - frequency=values[4], - frame_id=header.frame_id, - ts=header.stamp.sec + header.stamp.nsec / 1e9, - ) - - def __repr__(self) -> str: - return ( - f"ImuInfo(gyro_noise_density={self.gyro_noise_density}, " - f"gyro_random_walk={self.gyro_random_walk}, " - f"accel_noise_density={self.accel_noise_density}, " - f"accel_random_walk={self.accel_random_walk}, " - f"frequency={self.frequency}, frame_id='{self.frame_id}')" - ) diff --git a/dimos/msgs/sensor_msgs/JointCommand.py b/dimos/msgs/sensor_msgs/JointCommand.py deleted file mode 100644 index 78c541c50ec..00000000000 --- a/dimos/msgs/sensor_msgs/JointCommand.py +++ /dev/null @@ -1,143 +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. - -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -from io import BytesIO -import struct -import time - - -class JointCommand: - """ - Joint command message for robotic manipulators. - - Supports variable number of joints (DOF) with float64 values. - Can be used for position commands or velocity commands. - Includes timestamp for synchronization. - """ - - msg_name = "sensor_msgs.JointCommand" - - __slots__ = ["num_joints", "positions", "timestamp"] - - __typenames__ = ["double", "int32_t", "double"] - - __dimensions__ = [None, None, ["num_joints"]] - - def __init__( - self, positions: list[float] | None = None, timestamp: float | None = None - ) -> None: - """ - Initialize JointCommand. - - Args: - positions: List of joint values (positions or velocities) - timestamp: Unix timestamp (seconds since epoch). If None, uses current time. - """ - if positions is None: - positions = [] - - if timestamp is None: - timestamp = time.time() - - # LCM Type: double (timestamp) - self.timestamp = timestamp - # LCM Type: int32_t - self.num_joints = len(positions) - # LCM Type: double[num_joints] - self.positions = list(positions) - - def lcm_encode(self): # type: ignore[no-untyped-def] - """Encode for LCM transport (dimos uses lcm_encode method name).""" - return self.encode() # type: ignore[no-untyped-call] - - def encode(self): # type: ignore[no-untyped-def] - buf = BytesIO() - buf.write(JointCommand._get_packed_fingerprint()) # type: ignore[no-untyped-call] - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf) -> None: # type: ignore[no-untyped-def] - # Encode timestamp - buf.write(struct.pack(">d", self.timestamp)) - - # Encode num_joints - buf.write(struct.pack(">i", self.num_joints)) - - # Encode positions array - for i in range(self.num_joints): - buf.write(struct.pack(">d", self.positions[i])) - - @classmethod - def lcm_decode(cls, data: bytes): # type: ignore[no-untyped-def] - """Decode from LCM transport (dimos uses lcm_decode method name).""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes): # type: ignore[no-untyped-def] - if hasattr(data, "read"): - buf = data - else: - buf = BytesIO(data) # type: ignore[assignment] - if buf.read(8) != cls._get_packed_fingerprint(): # type: ignore[no-untyped-call] - raise ValueError("Decode error") - return cls._decode_one(buf) # type: ignore[no-untyped-call] - - @classmethod - def _decode_one(cls, buf): # type: ignore[no-untyped-def] - self = JointCommand.__new__(JointCommand) - - # Decode timestamp - self.timestamp = struct.unpack(">d", buf.read(8))[0] - - # Decode num_joints - self.num_joints = struct.unpack(">i", buf.read(4))[0] - - # Decode positions array - self.positions = [] - for _i in range(self.num_joints): - self.positions.append(struct.unpack(">d", buf.read(8))[0]) - - return self - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - # Hash for variable-length double array message - tmphash = (0x8A3D2E1C5F4B6A9D) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint = None - - @classmethod - def _get_packed_fingerprint(cls): # type: ignore[no-untyped-def] - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def get_hash(self): # type: ignore[no-untyped-def] - """Get the LCM hash of the struct""" - return struct.unpack(">Q", JointCommand._get_packed_fingerprint())[0] # type: ignore[no-untyped-call] - - def __str__(self) -> str: - return f"JointCommand(timestamp={self.timestamp:.6f}, num_joints={self.num_joints}, positions={self.positions})" - - def __repr__(self) -> str: - return f"JointCommand(positions={self.positions}, timestamp={self.timestamp})" diff --git a/dimos/msgs/sensor_msgs/JointState.py b/dimos/msgs/sensor_msgs/JointState.py deleted file mode 100644 index 3cbb74e0338..00000000000 --- a/dimos/msgs/sensor_msgs/JointState.py +++ /dev/null @@ -1,144 +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 time -from typing import Any, TypeAlias - -from dimos_lcm.sensor_msgs import JointState as LCMJointState - -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from JointState -JointStateConvertable: TypeAlias = dict[str, list[str] | list[float]] | LCMJointState - -_FIELDS = ("ts", "frame_id", "name", "position", "velocity", "effort") - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -def _fields_of(source: Any) -> tuple[Any, ...]: - """Pull the six JointState fields out of a dict, a JointState or an LCM message.""" - if isinstance(source, dict): - return ( - source.get("ts"), - source.get("frame_id", ""), - source.get("name"), - source.get("position"), - source.get("velocity"), - source.get("effort"), - ) - if isinstance(source, JointState): - return (source.ts, source.frame_id, *(list(getattr(source, f)) for f in _FIELDS[2:])) - return ( - source.header.stamp.sec + (source.header.stamp.nsec / 1_000_000_000), - source.header.frame_id, - *(list(getattr(source, f) or []) for f in _FIELDS[2:]), - ) - - -class JointState(Timestamped): - msg_name = "sensor_msgs.JointState" - ts: float - frame_id: str - name: list[str] - position: list[float] - velocity: list[float] - effort: list[float] - - def __init__( - self, - ts: float | JointStateConvertable | JointState | None = None, - frame_id: str = "", - name: list[str] | None = None, - position: list[float] | None = None, - velocity: list[float] | None = None, - effort: list[float] | None = None, - ) -> None: - """Initialize a JointState message. - - The first argument doubles as the source for the copy/dict/LCM forms: - ``JointState(other)``, ``JointState({...})``, ``JointState(lcm_msg)``. - - Args: - ts: Timestamp in seconds - frame_id: Frame ID for the message - name: List of joint names - position: List of joint positions (rad or m); gripper joints use - the unit their adapter declares - velocity: List of joint velocities (rad/s or m/s) - effort: List of joint efforts (Nm or N) - """ - stamp: Any = ts - if isinstance(stamp, dict | JointState | LCMJointState): - stamp, frame_id, name, position, velocity, effort = _fields_of(stamp) - - self.ts = time.time() if stamp is None else stamp - self.frame_id = frame_id - self.name = name if name is not None else [] - self.position = position if position is not None else [] - self.velocity = velocity if velocity is not None else [] - self.effort = effort if effort is not None else [] - - def lcm_encode(self) -> bytes: - lcm_msg = LCMJointState() - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - lcm_msg.name_length = len(self.name) - lcm_msg.name = self.name - lcm_msg.position_length = len(self.position) - lcm_msg.position = self.position - lcm_msg.velocity_length = len(self.velocity) - lcm_msg.velocity = self.velocity - lcm_msg.effort_length = len(self.effort) - lcm_msg.effort = self.effort - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> JointState: - lcm_msg = LCMJointState.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - name=list(lcm_msg.name) if lcm_msg.name else [], - position=list(lcm_msg.position) if lcm_msg.position else [], - velocity=list(lcm_msg.velocity) if lcm_msg.velocity else [], - effort=list(lcm_msg.effort) if lcm_msg.effort else [], - ) - - def __str__(self) -> str: - return f"JointState({len(self.name)} joints, frame_id='{self.frame_id}')" - - def __repr__(self) -> str: - return ( - f"JointState(ts={self.ts}, frame_id='{self.frame_id}', " - f"name={self.name}, position={self.position}, " - f"velocity={self.velocity}, effort={self.effort})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two JointState messages are equal.""" - if not isinstance(other, JointState): - return False - return ( - self.name == other.name - and self.position == other.position - and self.velocity == other.velocity - and self.effort == other.effort - and self.frame_id == other.frame_id - ) diff --git a/dimos/msgs/sensor_msgs/Joy.py b/dimos/msgs/sensor_msgs/Joy.py deleted file mode 100644 index 9443770b67e..00000000000 --- a/dimos/msgs/sensor_msgs/Joy.py +++ /dev/null @@ -1,130 +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 time -from typing import Any, TypeAlias - -from dimos_lcm.sensor_msgs import Joy as LCMJoy - -from dimos.types.timestamped import Timestamped - -# Types that can be converted to/from Joy -JoyConvertable: TypeAlias = ( - tuple[list[float], list[int]] | dict[str, list[float] | list[int]] | LCMJoy -) - - -def sec_nsec(ts): # type: ignore[no-untyped-def] - s = int(ts) - return [s, int((ts - s) * 1_000_000_000)] - - -def _fields_of(source: Any) -> tuple[Any, ...]: - """Pull the four Joy fields out of a dict, an (axes, buttons) pair, a Joy or an LCM message.""" - if isinstance(source, dict): - return ( - source.get("ts"), - source.get("frame_id", ""), - source.get("axes"), - source.get("buttons"), - ) - if isinstance(source, tuple | list): - return (None, "", list(source[0]), list(source[1])) - if isinstance(source, Joy): - return (source.ts, source.frame_id, list(source.axes), list(source.buttons)) - return ( - source.header.stamp.sec + (source.header.stamp.nsec / 1_000_000_000), - source.header.frame_id, - list(source.axes), - list(source.buttons), - ) - - -class Joy(Timestamped): - msg_name = "sensor_msgs.Joy" - ts: float - frame_id: str - axes: list[float] - buttons: list[int] - - def __init__( - self, - ts: float | JoyConvertable | Joy | None = None, - frame_id: str = "", - axes: list[float] | None = None, - buttons: list[int] | None = None, - ) -> None: - """Initialize a Joy message. - - The first argument doubles as the source for the copy/dict/pair/LCM forms: - ``Joy(other)``, ``Joy({...})``, ``Joy((axes, buttons))``, ``Joy(lcm_msg)``. - - Args: - ts: Timestamp in seconds - frame_id: Frame ID for the message - axes: List of axis values (typically -1.0 to 1.0) - buttons: List of button states (0 or 1) - """ - stamp: Any = ts - if isinstance(stamp, dict | tuple | list | Joy | LCMJoy): - stamp, frame_id, axes, buttons = _fields_of(stamp) - - self.ts = time.time() if stamp is None else stamp - self.frame_id = frame_id - self.axes = axes if axes is not None else [] - self.buttons = buttons if buttons is not None else [] - - def lcm_encode(self) -> bytes: - lcm_msg = LCMJoy() - [lcm_msg.header.stamp.sec, lcm_msg.header.stamp.nsec] = sec_nsec(self.ts) # type: ignore[no-untyped-call] - lcm_msg.header.frame_id = self.frame_id - lcm_msg.axes_length = len(self.axes) - lcm_msg.axes = self.axes - lcm_msg.buttons_length = len(self.buttons) - lcm_msg.buttons = self.buttons - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes) -> Joy: - lcm_msg = LCMJoy.lcm_decode(data) - return cls( - ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000), - frame_id=lcm_msg.header.frame_id, - axes=list(lcm_msg.axes) if lcm_msg.axes else [], - buttons=list(lcm_msg.buttons) if lcm_msg.buttons else [], - ) - - def __str__(self) -> str: - return ( - f"Joy(axes={len(self.axes)} values, buttons={len(self.buttons)} values, " - f"frame_id='{self.frame_id}')" - ) - - def __repr__(self) -> str: - return ( - f"Joy(ts={self.ts}, frame_id='{self.frame_id}', " - f"axes={self.axes}, buttons={self.buttons})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - """Check if two Joy messages are equal.""" - if not isinstance(other, Joy): - return False - return ( - self.axes == other.axes - and self.buttons == other.buttons - and self.frame_id == other.frame_id - ) diff --git a/dimos/msgs/sensor_msgs/MotorCommandArray.py b/dimos/msgs/sensor_msgs/MotorCommandArray.py deleted file mode 100644 index 577b03c7655..00000000000 --- a/dimos/msgs/sensor_msgs/MotorCommandArray.py +++ /dev/null @@ -1,133 +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. - -"""LCM type for low-level motor commands: q, dq, kp, kd, tau per motor.""" - -from io import BytesIO -import struct -import time - - -class MotorCommandArray: - """Per-motor q/dq/kp/kd/tau. Joint order is implicit (caller-defined).""" - - msg_name = "sensor_msgs.MotorCommandArray" - - __slots__ = ["dq", "kd", "kp", "num_joints", "q", "tau", "timestamp"] - - def __init__( - self, - q: list[float] | None = None, - dq: list[float] | None = None, - kp: list[float] | None = None, - kd: list[float] | None = None, - tau: list[float] | None = None, - timestamp: float | None = None, - ) -> None: - if q is None: - q = [] - if timestamp is None: - timestamp = time.time() - - n = len(q) - if dq is None: - dq = [0.0] * n - if kp is None: - kp = [0.0] * n - if kd is None: - kd = [0.0] * n - if tau is None: - tau = [0.0] * n - - if not (len(dq) == len(kp) == len(kd) == len(tau) == n): - raise ValueError( - f"All arrays must have length {n}; got " - f"dq={len(dq)} kp={len(kp)} kd={len(kd)} tau={len(tau)}" - ) - - self.timestamp = timestamp - self.num_joints = n - self.q = list(q) - self.dq = list(dq) - self.kp = list(kp) - self.kd = list(kd) - self.tau = list(tau) - - def lcm_encode(self) -> bytes: - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(MotorCommandArray._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - buf.write(struct.pack(">d", self.timestamp)) - buf.write(struct.pack(">i", self.num_joints)) - for arr in (self.q, self.dq, self.kp, self.kd, self.tau): - for v in arr: - buf.write(struct.pack(">d", v)) - - @classmethod - def lcm_decode(cls, data: bytes) -> "MotorCommandArray": - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes | BytesIO) -> "MotorCommandArray": - buf: BytesIO = data if isinstance(data, BytesIO) else BytesIO(data) - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error") - return cls._decode_one(buf) - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "MotorCommandArray": - self = MotorCommandArray.__new__(MotorCommandArray) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.num_joints = struct.unpack(">i", buf.read(4))[0] - n = self.num_joints - arrays = [] - for _ in range(5): - arrays.append([struct.unpack(">d", buf.read(8))[0] for _ in range(n)]) - self.q, self.dq, self.kp, self.kd, self.tau = arrays - return self - - @classmethod - def _get_hash_recursive(cls, parents: list[type]) -> int: - if cls in parents: - return 0 - # Distinct fingerprint from JointCommand (0x8A3D2E1C5F4B6A9D) - tmphash = (0x9B4E3F2D6A5C7B8E) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint: bytes | None = None - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) - return cls._packed_fingerprint - - def get_hash(self) -> int: - return int(struct.unpack(">Q", MotorCommandArray._get_packed_fingerprint())[0]) - - def __str__(self) -> str: - return f"MotorCommandArray(timestamp={self.timestamp:.6f}, num_joints={self.num_joints})" - - def __repr__(self) -> str: - return ( - f"MotorCommandArray(q={self.q}, dq={self.dq}, kp={self.kp}, " - f"kd={self.kd}, tau={self.tau}, timestamp={self.timestamp})" - ) diff --git a/dimos/msgs/sensor_msgs/PointCloud2.py b/dimos/msgs/sensor_msgs/PointCloud2.py deleted file mode 100644 index 9ec2bf6ea82..00000000000 --- a/dimos/msgs/sensor_msgs/PointCloud2.py +++ /dev/null @@ -1,1003 +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 functools -import struct -from typing import TYPE_CHECKING, Any - -# Import LCM types -from dimos_lcm.sensor_msgs.PointCloud2 import ( - PointCloud2 as LCMPointCloud2, -) -from dimos_lcm.sensor_msgs.PointField import PointField -from dimos_lcm.std_msgs.Header import Header -import numpy as np - -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.types.timestamped import Timestamped - -if TYPE_CHECKING: - import open3d as o3d # type: ignore[import-untyped] - from rerun._baseclasses import Archetype - - from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo - from dimos.msgs.sensor_msgs.Image import Image - - -@functools.lru_cache(maxsize=16) -def _get_matplotlib_cmap(name: str): # type: ignore[no-untyped-def] - """Get a matplotlib colormap by name (cached for performance).""" - import matplotlib.pyplot as plt - - return plt.get_cmap(name) - - -@functools.lru_cache(maxsize=16) -def _get_colormap_lut(name: str) -> np.ndarray: - """Build a 256-entry uint8 LUT from a matplotlib colormap (one-time cost).""" - cmap = _get_matplotlib_cmap(name) - t = np.linspace(0, 1, 256) - return (cmap(t)[:, :3] * 255).astype(np.uint8) # type: ignore[no-any-return] - - -def register_colormap_annotation(name: str = "turbo") -> None: - """Register a colormap as AnnotationContext so Rerun resolves colors viewer-side.""" - import rerun as rr - - lut = _get_colormap_lut(name) - rr.log( - "/", - rr.AnnotationContext( - [ - rr.datatypes.ClassDescription( - info=rr.datatypes.AnnotationInfo(id=i, color=lut[i].tolist()) - ) - for i in range(256) - ] - ), - static=True, - ) - - -# TODO: encode/decode need to be updated to work with full spectrum of pointcloud2 fields -class PointCloud2(Timestamped): - msg_name = "sensor_msgs.PointCloud2" - - def __init__( - self, - pointcloud: o3d.geometry.PointCloud | o3d.t.geometry.PointCloud | None = None, - frame_id: str = "world", - ts: float | None = None, - ) -> None: - import open3d as o3d # type: ignore[import-untyped] - - self.ts = ts # type: ignore[assignment] - self.frame_id = frame_id - - # Store internally as tensor pointcloud for speed - if pointcloud is None: - self._pcd_tensor: o3d.t.geometry.PointCloud = o3d.t.geometry.PointCloud() - elif isinstance(pointcloud, o3d.t.geometry.PointCloud): - self._pcd_tensor = pointcloud - elif len(pointcloud.points) == 0: - # from_legacy() warns on empty legacy clouds; build an empty tensor instead - self._pcd_tensor = o3d.t.geometry.PointCloud() - else: - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(pointcloud) - self._pcd_legacy_cache: o3d.geometry.PointCloud | None = None - - def _ensure_tensor_initialized(self) -> None: - """Ensure _pcd_tensor and _pcd_legacy_cache exist (handles unpickled old objects).""" - import open3d as o3d # type: ignore[import-untyped] - - # Always ensure _pcd_legacy_cache exists - if not hasattr(self, "_pcd_legacy_cache"): - self._pcd_legacy_cache = None - - # Check for old pickled format: 'pointcloud' directly in __dict__ - # This takes priority even if _pcd_tensor exists (it might be empty) - old_pcd = self.__dict__.get("pointcloud") - if old_pcd is not None and isinstance(old_pcd, o3d.geometry.PointCloud): - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(old_pcd) - self._pcd_legacy_cache = old_pcd # reuse it - del self.__dict__["pointcloud"] - return - - if not hasattr(self, "_pcd_tensor"): - self._pcd_tensor = o3d.t.geometry.PointCloud() - - def __getstate__(self) -> dict[str, object]: - """Serialize to numpy for pickling (tensors don't pickle well).""" - self._ensure_tensor_initialized() - state = self.__dict__.copy() - # Convert tensor to numpy for serialization - if "positions" in self._pcd_tensor.point: - state["_pcd_numpy"] = self._pcd_tensor.point["positions"].numpy() - else: - state["_pcd_numpy"] = np.zeros((0, 3), dtype=np.float32) - # Remove non-picklable objects - del state["_pcd_tensor"] - state["_pcd_legacy_cache"] = None - # Remove all cached_property entries - for key in list(state): - if isinstance(getattr(type(self), key, None), functools.cached_property): - del state[key] - return state - - def __setstate__(self, state: dict[str, object]) -> None: - """Restore from pickled state.""" - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - points_obj = state.pop("_pcd_numpy", None) - points: np.ndarray[tuple[int, int], np.dtype[np.float32]] = ( - points_obj if isinstance(points_obj, np.ndarray) else np.zeros((0, 3), dtype=np.float32) - ) - self.__dict__.update(state) - # Recreate tensor from numpy - self._pcd_tensor = o3d.t.geometry.PointCloud() - if len(points) > 0: - self._pcd_tensor.point["positions"] = o3c.Tensor(points, dtype=o3c.float32) - - @property - def pointcloud(self) -> o3d.geometry.PointCloud: - """Legacy pointcloud property for backwards compatibility. Cached.""" - self._ensure_tensor_initialized() - if self._pcd_legacy_cache is None: - self._pcd_legacy_cache = self._pcd_tensor.to_legacy() - return self._pcd_legacy_cache - - @pointcloud.setter - def pointcloud(self, value: o3d.geometry.PointCloud | o3d.t.geometry.PointCloud) -> None: - import open3d as o3d # type: ignore[import-untyped] - - if isinstance(value, o3d.t.geometry.PointCloud): - self._pcd_tensor = value - elif len(value.points) == 0: - self._pcd_tensor = o3d.t.geometry.PointCloud() - else: - self._pcd_tensor = o3d.t.geometry.PointCloud.from_legacy(value) - self._pcd_legacy_cache = None - - @property - def pointcloud_tensor(self) -> o3d.t.geometry.PointCloud: - """Direct access to tensor pointcloud (faster, no conversion).""" - self._ensure_tensor_initialized() - return self._pcd_tensor - - @classmethod - def from_numpy( - cls, - points: np.ndarray, - frame_id: str = "world", - timestamp: float | None = None, - intensities: np.ndarray | None = None, - offset_times: np.ndarray | None = None, - tags: np.ndarray | None = None, - lines: np.ndarray | None = None, - ) -> PointCloud2: - """Create PointCloud2 from numpy array of shape (N, 3). - - Args: - points: Nx3 numpy array of 3D points - frame_id: Frame ID for the point cloud - timestamp: Timestamp for the point cloud (defaults to current time) - intensities: Optional Nx1 or (N,) float array of per-point intensity values - offset_times: Optional (N,) uint32 array of per-point capture-time - offsets in nanoseconds relative to the header stamp - tags: Optional (N,) uint8 array of per-point sensor tag bytes - lines: Optional (N,) uint8 array of per-point laser line numbers - - Returns: - PointCloud2 instance - """ - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - for name, values in ( - ("intensities", intensities), - ("offset_times", offset_times), - ("tags", tags), - ("lines", lines), - ): - if values is not None and len(values) != len(points): - raise ValueError(f"{name} has {len(values)} entries for {len(points)} points") - pcd_t = o3d.t.geometry.PointCloud() - pcd_t.point["positions"] = o3c.Tensor(points.astype(np.float32), dtype=o3c.float32) - if intensities is not None: - arr = intensities.astype(np.float32) - if arr.ndim == 1: - arr = arr.reshape(-1, 1) - pcd_t.point["intensities"] = o3c.Tensor(arr, dtype=o3c.float32) - if offset_times is not None: - pcd_t.point["offset_times"] = o3c.Tensor( - offset_times.astype(np.uint32).reshape(-1, 1), dtype=o3c.uint32 - ) - if tags is not None: - pcd_t.point["tags"] = o3c.Tensor(tags.astype(np.uint8).reshape(-1, 1), dtype=o3c.uint8) - if lines is not None: - pcd_t.point["lines"] = o3c.Tensor( - lines.astype(np.uint8).reshape(-1, 1), dtype=o3c.uint8 - ) - return cls(pointcloud=pcd_t, ts=timestamp, frame_id=frame_id) - - @classmethod - def from_rgbd( - cls, - color_image: Image, - depth_image: Image, - camera_info: CameraInfo, - depth_scale: float = 1.0, - depth_trunc: float = 5.0, - ) -> PointCloud2: - """Create PointCloud2 from RGB and depth Image messages. - - Uses frame_id and timestamp from the depth image. - - Args: - color_image: RGB/BGR color Image message - depth_image: Depth Image message (float32 meters or uint16 mm) - camera_info: CameraInfo message with intrinsics - depth_scale: Scale factor to convert depth to meters (default 1.0 for float32) - depth_trunc: Maximum depth in meters to include - - Returns: - PointCloud2 instance with colored points - """ - import open3d as o3d # type: ignore[import-untyped] - - # Get color as RGB numpy array - color_data = color_image.to_rgb().data - if hasattr(color_data, "get"): # CuPy array - color_data = color_data.get() - color_data = np.ascontiguousarray(color_data) - - # Get depth numpy array - depth_data = depth_image.data - if hasattr(depth_data, "get"): # CuPy array - depth_data = depth_data.get() - - # Convert depth to float32 meters if needed - if depth_data.dtype == np.uint16: - depth_data = depth_data.astype(np.float32) * depth_scale - elif depth_data.dtype != np.float32: - depth_data = depth_data.astype(np.float32) - depth_data = np.ascontiguousarray(depth_data) - - # Verify dimensions match - color_h, color_w = color_data.shape[:2] - depth_h, depth_w = depth_data.shape[:2] - if (color_h, color_w) != (depth_h, depth_w): - raise ValueError( - f"Color {color_w}x{color_h} and depth {depth_w}x{depth_h} dimensions don't match" - ) - - # Get intrinsics from camera_info - intrinsic = camera_info.get_K_matrix() - fx, fy = intrinsic[0, 0], intrinsic[1, 1] - cx, cy = intrinsic[0, 2], intrinsic[1, 2] - - # Verify intrinsics match image dimensions - if camera_info.width != color_w or camera_info.height != color_h: - # Scale intrinsics if resolution differs - scale_x = color_w / camera_info.width - scale_y = color_h / camera_info.height - fx *= scale_x - fy *= scale_y - cx *= scale_x - cy *= scale_y - - # Create Open3D images - color_o3d = o3d.geometry.Image(color_data.astype(np.uint8)) - - # Filter invalid depth values - depth_filtered = depth_data.copy() - valid_mask = np.isfinite(depth_filtered) & (depth_filtered > 0) - depth_filtered[~valid_mask] = 0.0 - depth_o3d = o3d.geometry.Image(depth_filtered.astype(np.float32)) - - o3d_intrinsic = o3d.camera.PinholeCameraIntrinsic( - width=color_w, - height=color_h, - fx=fx, - fy=fy, - cx=cx, - cy=cy, - ) - - # Create RGBD image and point cloud - rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth( - color_o3d, - depth_o3d, - depth_scale=1.0, # Already scaled - depth_trunc=depth_trunc, - convert_rgb_to_intensity=False, - ) - - pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, o3d_intrinsic) - - return cls( - pointcloud=pcd, - frame_id=depth_image.frame_id, - ts=depth_image.ts, - ) - - def __str__(self) -> str: - return f"PointCloud2(frame_id='{self.frame_id}', num_points={len(self)})" - - @functools.cached_property - def center(self) -> Vector3: - """Calculate the center of the pointcloud in world frame.""" - center = np.asarray(self.pointcloud.points).mean(axis=0) - return Vector3(*center) - - def points(self): # type: ignore[no-untyped-def] - """Get points (returns tensor positions, use as_numpy() for numpy array).""" - import open3d.core as o3c # type: ignore[import-untyped] - - self._ensure_tensor_initialized() - if "positions" not in self._pcd_tensor.point: - return o3c.Tensor(np.zeros((0, 3), dtype=np.float32)) - return self._pcd_tensor.point["positions"] - - def __add__(self, other: PointCloud2) -> PointCloud2: - """Combine two PointCloud2 instances into one. - - The resulting point cloud contains points from both inputs. - The frame_id and timestamp are taken from the first point cloud. - - Args: - other: Another PointCloud2 instance to combine with - - Returns: - New PointCloud2 instance containing combined points - """ - if not isinstance(other, PointCloud2): - raise ValueError("Can only add PointCloud2 to another PointCloud2") - - return PointCloud2( - pointcloud=self.pointcloud + other.pointcloud, - frame_id=self.frame_id, - ts=max(self.ts, other.ts), - ) - - def transform(self, tf: Transform) -> PointCloud2: - """Transform the pointcloud using a Transform object. - - Applies the rotation and translation from the transform to all points, - converting them into the transform's frame_id. - - Args: - tf: Transform object containing rotation and translation - - Returns: - New PointCloud2 instance with transformed points in the new frame - """ - import open3d as o3d # type: ignore[import-untyped] - - points, _ = self.as_numpy() - - if len(points) == 0: - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=tf.frame_id, - ts=self.ts, - ) - - # Build 4x4 transformation matrix from Transform - transform_matrix = tf.to_matrix() - - # Convert points to homogeneous coordinates (N, 4) - ones = np.ones((len(points), 1)) - points_homogeneous = np.hstack([points, ones]) - - # Apply transformation: (4, 4) @ (4, N) -> (4, N) -> transpose to (N, 4) - transformed_points = (transform_matrix @ points_homogeneous.T).T - - # Extract xyz coordinates (drop homogeneous coordinate) - transformed_xyz = transformed_points[:, :3].astype(np.float64) - - # Create new Open3D point cloud - new_pcd = o3d.geometry.PointCloud() - new_pcd.points = o3d.utility.Vector3dVector(transformed_xyz) - - # Colors are frame-independent, carry them through. - if self.pointcloud.has_colors(): - new_pcd.colors = self.pointcloud.colors - - return PointCloud2( - pointcloud=new_pcd, - frame_id=tf.frame_id, - ts=self.ts, - ) - - def voxel_downsample(self, voxel_size: float = 0.025) -> PointCloud2: - """Downsample the pointcloud with a voxel grid.""" - if voxel_size <= 0: - return self - if len(self.pointcloud.points) < 20: - return self - downsampled = self._pcd_tensor.voxel_down_sample(voxel_size) - return PointCloud2(pointcloud=downsampled, frame_id=self.frame_id, ts=self.ts) - - def as_numpy( - self, - ) -> tuple[np.ndarray[Any, Any], np.ndarray[Any, Any] | None]: - """Get points and colors as numpy arrays. - - Returns: - Tuple of (points, colors) where: - - points: Nx3 numpy array of 3D points - - colors: Nx3 array in [0, 1] range, or None if no colors - """ - points = np.asarray(self.pointcloud.points) - colors = np.asarray(self.pointcloud.colors) if self.pointcloud.has_colors() else None - return points, colors - - def points_f32(self) -> np.ndarray: - """Get positions as float32 numpy array, bypassing legacy float64 conversion.""" - self._ensure_tensor_initialized() - if "positions" in self._pcd_tensor.point: - arr = self._pcd_tensor.point["positions"].numpy() - return arr.astype(np.float32) if arr.dtype != np.float32 else arr # type: ignore[no-any-return] - return np.zeros((0, 3), dtype=np.float32) - - def intensities_f32(self) -> np.ndarray | None: - """Get per-point intensity values as a flat float32 array, or None if absent.""" - self._ensure_tensor_initialized() - if "intensities" in self._pcd_tensor.point: - arr = self._pcd_tensor.point["intensities"].numpy().flatten() - return arr.astype(np.float32) if arr.dtype != np.float32 else arr # type: ignore[no-any-return] - return None - - def _per_point_field(self, name: str, np_dtype: type) -> np.ndarray | None: - """Read a per-point attribute from the tensor, or None if absent.""" - self._ensure_tensor_initialized() - if name not in self._pcd_tensor.point: - return None - arr = self._pcd_tensor.point[name].numpy().flatten() - return arr.astype(np_dtype) if arr.dtype != np_dtype else arr # type: ignore[no-any-return] - - def offset_times_u32(self) -> np.ndarray | None: - """Per-point time offsets (ns relative to header stamp) as flat uint32, or None.""" - return self._per_point_field("offset_times", np.uint32) - - def tags_u8(self) -> np.ndarray | None: - """Per-point sensor tag bytes as flat uint8, or None if absent.""" - return self._per_point_field("tags", np.uint8) - - def lines_u8(self) -> np.ndarray | None: - """Per-point laser line numbers as flat uint8, or None if absent.""" - return self._per_point_field("lines", np.uint8) - - @functools.cached_property - def axis_aligned_bounding_box(self) -> o3d.geometry.AxisAlignedBoundingBox: - """Get axis-aligned bounding box of the point cloud.""" - return self.pointcloud.get_axis_aligned_bounding_box() - - @functools.cached_property - def oriented_bounding_box(self) -> o3d.geometry.OrientedBoundingBox: - """Get oriented bounding box of the point cloud.""" - return self.pointcloud.get_oriented_bounding_box() - - @functools.cached_property - def bounding_box_dimensions(self) -> tuple[float, float, float]: - """Get dimensions (width, height, depth) of axis-aligned bounding box.""" - bbox = self.axis_aligned_bounding_box - extent = bbox.get_extent() - return tuple(extent) - - def bounding_box_intersects(self, other: PointCloud2) -> bool: - # Get axis-aligned bounding boxes - bbox1 = self.axis_aligned_bounding_box - bbox2 = other.axis_aligned_bounding_box - - # Get min and max bounds - min1 = bbox1.get_min_bound() - max1 = bbox1.get_max_bound() - min2 = bbox2.get_min_bound() - max2 = bbox2.get_max_bound() - - # Check overlap in all three dimensions - # Boxes intersect if they overlap in ALL dimensions - return ( # type: ignore[no-any-return] - min1[0] <= max2[0] - and max1[0] >= min2[0] - and min1[1] <= max2[1] - and max1[1] >= min2[1] - and min1[2] <= max2[2] - and max1[2] >= min2[2] - ) - - def lcm_encode(self, frame_id: str | None = None) -> bytes: - """Convert to LCM PointCloud2 message with optional RGB colors.""" - msg = LCMPointCloud2() - - # Header - msg.header = Header() - msg.header.seq = 0 - msg.header.frame_id = frame_id or self.frame_id - - msg.header.stamp.sec = int(self.ts) - msg.header.stamp.nsec = int((self.ts - int(self.ts)) * 1e9) - - points, _ = self.as_numpy() - - # Check if pointcloud has colors - self._ensure_tensor_initialized() - has_colors = "colors" in self._pcd_tensor.point - - if len(points) == 0: - msg.height = 0 - msg.width = 0 - msg.point_step = 16 - msg.row_step = 0 - msg.data_length = 0 - msg.data = b"" - msg.is_dense = True - msg.is_bigendian = False - msg.fields_length = 4 - msg.fields = self._create_xyzrgb_fields() if has_colors else self._create_xyz_fields() - return msg.lcm_encode() # type: ignore[no-any-return] - - msg.height = 1 - msg.width = len(points) - - if has_colors: - # Get colors (0-1 range) and convert to uint8 - colors = self._pcd_tensor.point["colors"].numpy() - if colors.max() <= 1.0: - colors = (colors * 255).astype(np.uint8) - else: - colors = colors.astype(np.uint8) - - # Pack RGB into float32 (ROS convention: bytes are [padding, r, g, b]) - rgb_packed = np.zeros(len(points), dtype=np.float32) - rgb_uint32 = ( - (colors[:, 0].astype(np.uint32) << 16) - | (colors[:, 1].astype(np.uint32) << 8) - | colors[:, 2].astype(np.uint32) - ) - rgb_packed = rgb_uint32.view(np.float32) - - msg.fields = self._create_xyzrgb_fields() - msg.fields_length = 4 - msg.point_step = 16 # x, y, z, rgb (4 floats) - - point_data = np.column_stack([points, rgb_packed]).astype(np.float32) - else: - msg.fields = self._create_xyz_fields() - msg.fields_length = 4 - msg.point_step = 16 # x, y, z, intensity - - if "intensities" in self._pcd_tensor.point: - intensities = ( - self._pcd_tensor.point["intensities"].numpy().flatten().astype(np.float32) - ) - else: - intensities = np.zeros(len(points), dtype=np.float32) - - point_data = np.column_stack([points, intensities]).astype(np.float32) - - # Optional per-point attributes (offset_time/tag/line) extend the - # base 16-byte layout with packed extra fields, emitted only when present. - extras: list[tuple[str, np.dtype, int, np.ndarray]] = [] - offset_times = self.offset_times_u32() - if offset_times is not None: - extras.append(("offset_time", np.dtype(" None: - """Preload the heavy imports lcm_decode needs. - - Called at subscribe time (see LCMEncoderMixin.subscribe) so the first - decode doesn't stall the LCM handler thread on the open3d import. - """ - import open3d.core # type: ignore[import-untyped] # noqa: F401 - - @classmethod - def lcm_decode(cls, data: bytes) -> PointCloud2: - import open3d as o3d # type: ignore[import-untyped] - import open3d.core as o3c # type: ignore[import-untyped] - - msg = LCMPointCloud2.lcm_decode(data) - - if msg.width == 0 or msg.height == 0: - pc = o3d.geometry.PointCloud() - return cls( - pointcloud=pc, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") and msg.header.stamp.sec > 0 - else None, - ) - - # Parse field offsets. The message is self-describing; a known field is - # honored only when its advertised datatype matches what we read it as, - # otherwise it is treated as absent rather than misread. - _expected_datatype = { - "x": PointField.FLOAT32, - "y": PointField.FLOAT32, - "z": PointField.FLOAT32, - "rgb": PointField.FLOAT32, - "intensity": PointField.FLOAT32, - "offset_time": PointField.UINT32, - "tag": PointField.UINT8, - "line": PointField.UINT8, - } - x_offset = y_offset = z_offset = rgb_offset = intensity_offset = None - offset_time_offset = tag_offset = line_offset = None - for msgfield in msg.fields: - if _expected_datatype.get(msgfield.name) != msgfield.datatype: - continue - if msgfield.name == "x": - x_offset = msgfield.offset - elif msgfield.name == "y": - y_offset = msgfield.offset - elif msgfield.name == "z": - z_offset = msgfield.offset - elif msgfield.name == "rgb": - rgb_offset = msgfield.offset - elif msgfield.name == "intensity": - intensity_offset = msgfield.offset - elif msgfield.name == "offset_time": - offset_time_offset = msgfield.offset - elif msgfield.name == "tag": - tag_offset = msgfield.offset - elif msgfield.name == "line": - line_offset = msgfield.offset - - if any(offset is None for offset in [x_offset, y_offset, z_offset]): - raise ValueError("PointCloud2 message missing X, Y, or Z msgfields") - - num_points = msg.width * msg.height - raw_data = msg.data - point_step = msg.point_step - - # Fast path for standard layout - if x_offset == 0 and y_offset == 4 and z_offset == 8 and point_step >= 12: - if point_step == 12: - points = np.frombuffer(raw_data, dtype=np.float32).reshape(-1, 3) - else: - dt = np.dtype( - [("x", " np.ndarray: - item_size = np.dtype(np_dtype).itemsize - dt_s = np.dtype( - [ - ("_pre", f"V{field_offset}"), - ("value", np_dtype), - ("_post", f"V{point_step - field_offset - item_size}"), - ] - ) - structured_s = np.frombuffer(raw_data, dtype=dt_s, count=num_points) - return np.ascontiguousarray(structured_s["value"]) - - if offset_time_offset is not None: - pcd_t.point["offset_times"] = o3c.Tensor( - _extract_scalar_field(offset_time_offset, "> 16) & 0xFF).astype(np.float32) / 255.0 - g = ((rgb_packed >> 8) & 0xFF).astype(np.float32) / 255.0 - b = (rgb_packed & 0xFF).astype(np.float32) / 255.0 - colors = np.column_stack([r, g, b]) - pcd_t.point["colors"] = o3c.Tensor(colors, dtype=o3c.float32) - - return cls( - pointcloud=pcd_t, - frame_id=msg.header.frame_id if hasattr(msg, "header") else "", - ts=msg.header.stamp.sec + msg.header.stamp.nsec / 1e9 - if hasattr(msg, "header") and msg.header.stamp.sec > 0 - else None, - ) - - def _create_xyz_fields(self) -> list: # type: ignore[type-arg] - """Create X, Y, Z, intensity field definitions.""" - fields = [] - for i, name in enumerate(["x", "y", "z", "intensity"]): - field = PointField() - field.name = name - field.offset = i * 4 - field.datatype = 7 # FLOAT32 - field.count = 1 - fields.append(field) - return fields - - def _create_xyzrgb_fields(self) -> list: # type: ignore[type-arg] - """Create X, Y, Z, RGB field definitions for colored pointclouds.""" - fields = [] - for i, name in enumerate(["x", "y", "z"]): - field = PointField() - field.name = name - field.offset = i * 4 - field.datatype = 7 # FLOAT32 - field.count = 1 - fields.append(field) - - # RGB field (packed as float32, ROS convention) - rgb_field = PointField() - rgb_field.name = "rgb" - rgb_field.offset = 12 - rgb_field.datatype = 7 # FLOAT32 (contains packed RGB) - rgb_field.count = 1 - fields.append(rgb_field) - - return fields - - def __len__(self) -> int: - """Return number of points.""" - self._ensure_tensor_initialized() - if "positions" not in self._pcd_tensor.point: - return 0 - return int(self._pcd_tensor.point["positions"].shape[0]) - - def to_rerun( - self, - voxel_size: float = 0.05, - colors: list[int] | None = None, - mode: str = "spheres", - fill_mode: str = "solid", - bottom_cutoff: float | None = None, - ui_radius: float = 2.0, - rgb: bool = True, - **kwargs: object, - ) -> Archetype: - """Convert to Rerun archetype for visualization. - - Args: - voxel_size: size for visualization - colors: Optional RGB color [r, g, b] for all points (0-255). - If None, uses height-based turbo colormap via class_ids - (requires register_colormap_annotation() called once). - mode: "points" for flat screen-space dots, "boxes" for cubes, or - "spheres" (default) for world-sized spheres. Only "points" holds a - constant on-screen size as you zoom; the others scale with voxel_size. - fill_mode: Fill mode for boxes - "solid", "majorwireframe", or "densewireframe" - ui_radius: Dot radius in screen-space UI points; "points" mode only. - rgb: Paint with the cloud's own per-point colors when it has any (an - RGBD cloud); off, or on a colorless cloud, the height colormap. - **kwargs: Additional args (ignored for compatibility) - - Returns: - rr.Points3D or rr.Boxes3D archetype for logging to Rerun - """ - import rerun as rr - - points = self.points_f32() - if len(points) == 0: - return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) - - if bottom_cutoff is not None: - points = points[points[:, 2] >= bottom_cutoff] - if len(points) == 0: - return rr.Points3D([]) if mode != "boxes" else rr.Boxes3D(centers=[]) - - # Use class_ids for height-based colormap (viewer resolves colors via AnnotationContext) - # Fall back to explicit colors when provided - class_ids = None - point_colors: Any = None - if colors is not None: - point_colors = colors - elif rgb and self.pointcloud.has_colors(): - own = np.asarray(self.pointcloud.colors) - if bottom_cutoff is not None: - own = own[np.asarray(self.pointcloud.points)[:, 2] >= bottom_cutoff] - point_colors = (own * 255).astype(np.uint8) - else: - z = points[:, 2] - class_ids = ((z - z.min()) / (z.max() - z.min() + 1e-8) * 255).astype(np.uint8) - - if mode == "points": - # Negative radii are screen-space UI points in rerun. - return rr.Points3D( - positions=points, colors=point_colors, class_ids=class_ids, radii=-ui_radius - ) - elif mode == "boxes": - half = voxel_size / 2 - return rr.Boxes3D( - centers=points, - half_sizes=[half, half, half], - colors=point_colors, - class_ids=class_ids, - fill_mode=fill_mode, # type: ignore[arg-type] - ) - else: - return rr.Points3D( - positions=points, - radii=voxel_size / 2, - colors=point_colors, - class_ids=class_ids, - ) - - def filter_by_height( - self, - min_height: float | None = None, - max_height: float | None = None, - ) -> PointCloud2: - """Filter points based on their height (z-coordinate). - - This method creates a new PointCloud2 containing only points within the specified - height range. All metadata (frame_id, timestamp) is preserved. - - Args: - min_height: Optional minimum height threshold. Points with z < min_height are filtered out. - If None, no lower limit is applied. - max_height: Optional maximum height threshold. Points with z > max_height are filtered out. - If None, no upper limit is applied. - - Returns: - New PointCloud2 instance containing only the filtered points. - - Raises: - ValueError: If both min_height and max_height are None (no filtering would occur). - - Example: - # Remove ground points below 0.1m height - filtered_pc = pointcloud.filter_by_height(min_height=0.1) - - # Keep only points between ground level and 2m height - filtered_pc = pointcloud.filter_by_height(min_height=0.0, max_height=2.0) - - # Remove points above 1.5m (e.g., ceiling) - filtered_pc = pointcloud.filter_by_height(max_height=1.5) - """ - import open3d as o3d # type: ignore[import-untyped] - - # Validate that at least one threshold is provided - if min_height is None and max_height is None: - raise ValueError("At least one of min_height or max_height must be specified") - - # Get points as numpy array - points, _ = self.as_numpy() - - if len(points) == 0: - # Empty pointcloud - return a copy - return PointCloud2( - pointcloud=o3d.geometry.PointCloud(), - frame_id=self.frame_id, - ts=self.ts, - ) - - # Extract z-coordinates (height values) - column index 2 - heights = points[:, 2] - - # Create boolean mask for filtering based on height thresholds - # Start with all True values - mask = np.ones(len(points), dtype=bool) - - # Apply minimum height filter if specified - if min_height is not None: - mask &= heights >= min_height - - # Apply maximum height filter if specified - if max_height is not None: - mask &= heights <= max_height - - # Apply mask to filter points - filtered_points = points[mask] - - # Create new PointCloud2 with filtered points - return PointCloud2.from_numpy( - points=filtered_points, - frame_id=self.frame_id, - timestamp=self.ts, - ) - - def __repr__(self) -> str: - """String representation.""" - return f"PointCloud(points={len(self)}, frame_id='{self.frame_id}', ts={self.ts})" diff --git a/dimos/msgs/sensor_msgs/RobotState.py b/dimos/msgs/sensor_msgs/RobotState.py deleted file mode 100644 index 20e41e7d248..00000000000 --- a/dimos/msgs/sensor_msgs/RobotState.py +++ /dev/null @@ -1,188 +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. - -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -from io import BytesIO -import struct - - -class RobotState: - msg_name = "sensor_msgs.RobotState" - - __slots__ = [ - "cmdnum", - "error_code", - "joints", - "mode", - "mt_able", - "mt_brake", - "state", - "tcp_offset", - "tcp_pose", - "warn_code", - ] - - __typenames__ = [ - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "int32_t", - "float", - "float", - "float", - ] - - __dimensions__ = [None, None, None, None, None, None, None, None, None, None] - - def __init__( # type: ignore[no-untyped-def] - self, - state: int = 0, - mode: int = 0, - error_code: int = 0, - warn_code: int = 0, - cmdnum: int = 0, - mt_brake: int = 0, - mt_able: int = 0, - tcp_pose=None, - tcp_offset=None, - joints=None, - ) -> None: - # LCM Type: int32_t - self.state = state - # LCM Type: int32_t - self.mode = mode - # LCM Type: int32_t - self.error_code = error_code - # LCM Type: int32_t - self.warn_code = warn_code - # LCM Type: int32_t - self.cmdnum = cmdnum - # LCM Type: int32_t - self.mt_brake = mt_brake - # LCM Type: int32_t - self.mt_able = mt_able - # LCM Type: float[] - TCP pose [x, y, z, roll, pitch, yaw] - self.tcp_pose = tcp_pose if tcp_pose is not None else [] - # LCM Type: float[] - TCP offset [x, y, z, roll, pitch, yaw] - self.tcp_offset = tcp_offset if tcp_offset is not None else [] - # LCM Type: float[] - Joint positions (variable length based on robot DOF) - self.joints = joints if joints is not None else [] - - def lcm_encode(self): # type: ignore[no-untyped-def] - """Encode for LCM transport (dimos uses lcm_encode method name).""" - return self.encode() # type: ignore[no-untyped-call] - - def encode(self): # type: ignore[no-untyped-def] - buf = BytesIO() - buf.write(RobotState._get_packed_fingerprint()) # type: ignore[no-untyped-call] - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf) -> None: # type: ignore[no-untyped-def] - buf.write( - struct.pack( - ">iiiiiii", - self.state, - self.mode, - self.error_code, - self.warn_code, - self.cmdnum, - self.mt_brake, - self.mt_able, - ) - ) - # Encode tcp_pose array - buf.write(struct.pack(">i", len(self.tcp_pose))) - for val in self.tcp_pose: - buf.write(struct.pack(">f", val)) - # Encode tcp_offset array - buf.write(struct.pack(">i", len(self.tcp_offset))) - for val in self.tcp_offset: - buf.write(struct.pack(">f", val)) - # Encode joints array - buf.write(struct.pack(">i", len(self.joints))) - for val in self.joints: - buf.write(struct.pack(">f", val)) - - @classmethod - def lcm_decode(cls, data: bytes): # type: ignore[no-untyped-def] - """Decode from LCM transport (dimos uses lcm_decode method name).""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes): # type: ignore[no-untyped-def] - if hasattr(data, "read"): - buf = data - else: - buf = BytesIO(data) # type: ignore[assignment] - if buf.read(8) != cls._get_packed_fingerprint(): # type: ignore[no-untyped-call] - raise ValueError("Decode error") - return cls._decode_one(buf) # type: ignore[no-untyped-call] - - @classmethod - def _decode_one(cls, buf): # type: ignore[no-untyped-def] - self = RobotState() - ( - self.state, - self.mode, - self.error_code, - self.warn_code, - self.cmdnum, - self.mt_brake, - self.mt_able, - ) = struct.unpack(">iiiiiii", buf.read(28)) - # Decode tcp_pose array - tcp_pose_len = struct.unpack(">i", buf.read(4))[0] - self.tcp_pose = [] - for _ in range(tcp_pose_len): - self.tcp_pose.append(struct.unpack(">f", buf.read(4))[0]) - # Decode tcp_offset array - tcp_offset_len = struct.unpack(">i", buf.read(4))[0] - self.tcp_offset = [] - for _ in range(tcp_offset_len): - self.tcp_offset.append(struct.unpack(">f", buf.read(4))[0]) - # Decode joints array - joints_len = struct.unpack(">i", buf.read(4))[0] - self.joints = [] - for _ in range(joints_len): - self.joints.append(struct.unpack(">f", buf.read(4))[0]) - return self - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - # Updated hash to reflect new fields: tcp_pose, tcp_offset, joints - tmphash = (0x8C3B9A1FE7D24E6A) & 0xFFFFFFFFFFFFFFFF - tmphash = (((tmphash << 1) & 0xFFFFFFFFFFFFFFFF) + (tmphash >> 63)) & 0xFFFFFFFFFFFFFFFF - return tmphash - - _packed_fingerprint = None - - @classmethod - def _get_packed_fingerprint(cls): # type: ignore[no-untyped-def] - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def get_hash(self): # type: ignore[no-untyped-def] - """Get the LCM hash of the struct""" - return struct.unpack(">Q", RobotState._get_packed_fingerprint())[0] # type: ignore[no-untyped-call] diff --git a/dimos/msgs/sensor_msgs/image_impls/AbstractImage.py b/dimos/msgs/sensor_msgs/image_impls/AbstractImage.py deleted file mode 100644 index 3b901b874b8..00000000000 --- a/dimos/msgs/sensor_msgs/image_impls/AbstractImage.py +++ /dev/null @@ -1,17 +0,0 @@ -# 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. - -# Backwards compatibility stub for unpickling old data. -# AbstractImage and ImageFormat were moved to Image. -from dimos.msgs.sensor_msgs.Image import Image as AbstractImage, ImageFormat # noqa: F401 diff --git a/dimos/msgs/std_msgs/Bool.py b/dimos/msgs/std_msgs/Bool.py deleted file mode 100644 index 4421447edfc..00000000000 --- a/dimos/msgs/std_msgs/Bool.py +++ /dev/null @@ -1,28 +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. - -"""Bool message type.""" - -from dimos_lcm.std_msgs import Bool as LCMBool - - -class Bool(LCMBool): # type: ignore[misc] - """Bool message.""" - - msg_name = "std_msgs.Bool" - - def __init__(self, data: bool = False) -> None: - """Initialize Bool with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Float32.py b/dimos/msgs/std_msgs/Float32.py deleted file mode 100644 index f5a508d21c0..00000000000 --- a/dimos/msgs/std_msgs/Float32.py +++ /dev/null @@ -1,30 +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. - -"""Float32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Float32 as LCMFloat32 - - -class Float32(LCMFloat32): # type: ignore[misc] - """Float32 message.""" - - msg_name: ClassVar[str] = "std_msgs.Float32" - - def __init__(self, data: float = 0.0) -> None: - """Initialize Float32 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Header.py b/dimos/msgs/std_msgs/Header.py deleted file mode 100644 index 5c542004972..00000000000 --- a/dimos/msgs/std_msgs/Header.py +++ /dev/null @@ -1,106 +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 datetime import datetime -import time - -from dimos_lcm.std_msgs import Header as LCMHeader, Time as LCMTime -from plum import dispatch - -# Import the actual LCM header type that's returned from decoding -try: - from lcm_msgs.std_msgs.Header import ( # type: ignore[import-not-found] - Header as DecodedLCMHeader, - ) -except ImportError: - DecodedLCMHeader = None - - -class Header(LCMHeader): # type: ignore[misc] - msg_name = "std_msgs.Header" - ts: float - - @dispatch - def __init__(self) -> None: - """Initialize a Header with current time and empty frame_id.""" - self.ts = time.time() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=0, stamp=LCMTime(sec=sec, nsec=nsec), frame_id="") - - @dispatch # type: ignore[no-redef] - def __init__(self, frame_id: str) -> None: - """Initialize a Header with current time and specified frame_id.""" - self.ts = time.time() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, timestamp: float, frame_id: str = "", seq: int = 1) -> None: - """Initialize a Header with Unix timestamp, frame_id, and optional seq.""" - sec = int(timestamp) - nsec = int((timestamp - sec) * 1_000_000_000) - super().__init__(seq=seq, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, timestamp: datetime, frame_id: str = "") -> None: - """Initialize a Header with datetime object and frame_id.""" - self.ts = timestamp.timestamp() - sec = int(self.ts) - nsec = int((self.ts - sec) * 1_000_000_000) - super().__init__(seq=1, stamp=LCMTime(sec=sec, nsec=nsec), frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, seq: int, stamp: LCMTime, frame_id: str) -> None: - """Initialize with explicit seq, stamp, and frame_id (LCM compatibility).""" - super().__init__(seq=seq, stamp=stamp, frame_id=frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, header: LCMHeader) -> None: - """Initialize from another Header (copy constructor).""" - super().__init__(seq=header.seq, stamp=header.stamp, frame_id=header.frame_id) - - @dispatch # type: ignore[no-redef] - def __init__(self, header: object) -> None: - """Initialize from a decoded LCM header object.""" - # Handle the case where we get an lcm_msgs.std_msgs.Header.Header object - if hasattr(header, "seq") and hasattr(header, "stamp") and hasattr(header, "frame_id"): - super().__init__(seq=header.seq, stamp=header.stamp, frame_id=header.frame_id) - else: - raise ValueError(f"Cannot create Header from {type(header)}") - - @classmethod - def now(cls, frame_id: str = "", seq: int = 1) -> Header: - """Create a Header with current timestamp.""" - ts = time.time() - return cls(ts, frame_id, seq) - - @property - def timestamp(self) -> float: - """Get timestamp as Unix time (float).""" - return self.stamp.sec + (self.stamp.nsec / 1_000_000_000) # type: ignore[no-any-return] - - @property - def datetime(self) -> datetime: - """Get timestamp as datetime object.""" - return datetime.fromtimestamp(self.timestamp) - - def __str__(self) -> str: - return f"Header(seq={self.seq}, time={self.timestamp:.6f}, frame_id='{self.frame_id}')" - - def __repr__(self) -> str: - return f"Header(seq={self.seq}, stamp=Time(sec={self.stamp.sec}, nsec={self.stamp.nsec}), frame_id='{self.frame_id}')" diff --git a/dimos/msgs/std_msgs/Int32.py b/dimos/msgs/std_msgs/Int32.py deleted file mode 100644 index ba4906f485d..00000000000 --- a/dimos/msgs/std_msgs/Int32.py +++ /dev/null @@ -1,32 +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. - -# Copyright 2025-2026 Dimensional Inc. - -"""Int32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Int32 as LCMInt32 - - -class Int32(LCMInt32): # type: ignore[misc] - """ROS-compatible Int32 message.""" - - msg_name: ClassVar[str] = "std_msgs.Int32" - - def __init__(self, data: int = 0) -> None: - """Initialize Int32 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/Int8.py b/dimos/msgs/std_msgs/Int8.py deleted file mode 100644 index ca871403534..00000000000 --- a/dimos/msgs/std_msgs/Int8.py +++ /dev/null @@ -1,32 +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. - -# Copyright 2025-2026 Dimensional Inc. - -"""Int32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import Int8 as LCMInt8 - - -class Int8(LCMInt8): # type: ignore[misc] - """Int8 message.""" - - msg_name: ClassVar[str] = "std_msgs.Int8" - - def __init__(self, data: int = 0) -> None: - """Initialize Int8 with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/String.py b/dimos/msgs/std_msgs/String.py deleted file mode 100644 index ef48233dfd3..00000000000 --- a/dimos/msgs/std_msgs/String.py +++ /dev/null @@ -1,30 +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. - -"""String message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import String as LCMString - - -class String(LCMString): # type: ignore[misc] - """ROS-compatible String message.""" - - msg_name: ClassVar[str] = "std_msgs.String" - - def __init__(self, data: str = "") -> None: - """Initialize String with data value.""" - self.data = data diff --git a/dimos/msgs/std_msgs/UInt32.py b/dimos/msgs/std_msgs/UInt32.py deleted file mode 100644 index e617c782fed..00000000000 --- a/dimos/msgs/std_msgs/UInt32.py +++ /dev/null @@ -1,30 +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. - -"""UInt32 message type.""" - -from typing import ClassVar - -from dimos_lcm.std_msgs import UInt32 as LCMUInt32 - - -class UInt32(LCMUInt32): # type: ignore[misc] - """ROS-compatible UInt32 message.""" - - msg_name: ClassVar[str] = "std_msgs.UInt32" - - def __init__(self, data: int = 0) -> None: - """Initialize UInt32 with data value.""" - self.data = data diff --git a/dimos/msgs/tf2_msgs/TFMessage.py b/dimos/msgs/tf2_msgs/TFMessage.py deleted file mode 100644 index 5e44b7762d9..00000000000 --- a/dimos/msgs/tf2_msgs/TFMessage.py +++ /dev/null @@ -1,194 +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 typing import TYPE_CHECKING, BinaryIO - -from dimos_lcm.tf2_msgs import TFMessage as LCMTFMessage - -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 - -if TYPE_CHECKING: - from collections.abc import Iterable, Iterator - - from dimos.visualization.rerun.bridge import RerunMulti - -DEFAULT_LINKS_ROOT = "tf_links" -DEFAULT_TF_ROOT = "world/tf" -DEFAULT_AXIS_LENGTH = 0.5 -DEPTH_SCALE = 0.8 -AXIS_WIDTH_UI_POINTS = 2.0 -AXIS_COLORS = [[255, 0, 0], [0, 255, 0], [0, 0, 255]] - - -def _triad(length: float): # type: ignore[no-untyped-def] - """XYZ arrows, red green blue.""" - import rerun as rr - - return rr.Arrows3D( - origins=[[0.0, 0.0, 0.0]] * 3, - vectors=[[length, 0.0, 0.0], [0.0, length, 0.0], [0.0, 0.0, length]], - colors=AXIS_COLORS, - radii=rr.components.Radius.ui_points(AXIS_WIDTH_UI_POINTS), - ) - - -class TfFrameTree: - """Store each frame under its parents. This lets us view the tree in the left panel.""" - - def __init__( - self, axis_length: float = DEFAULT_AXIS_LENGTH, root: str = DEFAULT_TF_ROOT - ) -> None: - self.axis_length = axis_length - self.root = root - self._parents: dict[str, str] = {} - self._placed: dict[str, tuple[str, int]] = {} # frame -> (path, depth) - - def placements(self) -> dict[str, str]: - return {frame: path for frame, (path, _depth) in self._placed.items()} - - def update(self, transforms: Iterable[Transform]) -> None: - learned = False - for transform in transforms: - if self._parents.get(transform.child_frame_id) != transform.frame_id: - self._parents[transform.child_frame_id] = transform.frame_id - learned = True - if learned: - self._redraw() - - def _place(self, frame: str, walked: frozenset[str]) -> tuple[str, int]: - import rerun as rr - - part = rr.escape_entity_path_part(frame) - parent = self._parents.get(frame) - if parent is None or parent in walked: - return f"{self.root}/{part}", 0 - parent_path, parent_depth = self._place(parent, walked | {frame}) - return f"{parent_path}/{part}", parent_depth + 1 - - def _redraw(self) -> None: - import rerun as rr - - frames = {*self._parents, *self._parents.values()} - placed = {frame: self._place(frame, frozenset()) for frame in frames} - - for frame, (old_path, _depth) in self._placed.items(): - new = placed.get(frame) - if new is None or new[0] != old_path: - rr.log(old_path, rr.Arrows3D(origins=[], vectors=[]), static=True) - - for frame, (path, depth) in placed.items(): - if self._placed.get(frame) != (path, depth): - rr.log( - path, - rr.CoordinateFrame(f"tf#/{frame}"), - _triad(self.axis_length * DEPTH_SCALE**depth), - static=True, - ) - - self._placed = placed - - -class TFMessage: - """TFMessage that accepts Transform objects and encodes to LCM format.""" - - transforms: list[Transform] - msg_name = "tf2_msgs.TFMessage" - - def __init__(self, *transforms: Transform) -> None: - self.transforms = list(transforms) - - def add_transform(self, transform: Transform, child_frame_id: str = "base_link") -> None: - """Add a transform to the message.""" - self.transforms.append(transform) - self.transforms_length = len(self.transforms) - - def lcm_encode(self) -> bytes: - """Encode as LCM TFMessage.""" - res = [t.lcm_transform() for t in self.transforms] - - lcm_msg = LCMTFMessage( - transforms_length=len(self.transforms), - transforms=res, - ) - - return lcm_msg.lcm_encode() # type: ignore[no-any-return] - - @classmethod - def lcm_decode(cls, data: bytes | BinaryIO) -> TFMessage: - """Decode from LCM TFMessage bytes.""" - lcm_msg = LCMTFMessage.lcm_decode(data) - - transforms = [] - for lcm_transform_stamped in lcm_msg.transforms: - ts = lcm_transform_stamped.header.stamp.sec + ( - lcm_transform_stamped.header.stamp.nsec / 1_000_000_000 - ) - - lcm_trans = lcm_transform_stamped.transform.translation - lcm_rot = lcm_transform_stamped.transform.rotation - - transform = Transform( - translation=Vector3(lcm_trans.x, lcm_trans.y, lcm_trans.z), - rotation=Quaternion(lcm_rot.x, lcm_rot.y, lcm_rot.z, lcm_rot.w), - frame_id=lcm_transform_stamped.header.frame_id, - child_frame_id=lcm_transform_stamped.child_frame_id, - ts=ts, - ) - transforms.append(transform) - - return cls(*transforms) - - def __len__(self) -> int: - """Return number of transforms.""" - return len(self.transforms) - - def __getitem__(self, index: int) -> Transform: - """Get transform by index.""" - return self.transforms[index] - - def __iter__(self) -> Iterator: # type: ignore[type-arg] - """Iterate over transforms.""" - return iter(self.transforms) - - def __repr__(self) -> str: - return f"TFMessage({len(self.transforms)} transforms)" - - def __str__(self) -> str: - lines = [f"TFMessage with {len(self.transforms)} transforms:"] - for i, transform in enumerate(self.transforms): - lines.append( - f" [{i}] {transform.frame_id} → {transform.child_frame_id} @ {transform.ts:.3f}" - ) - return "\n".join(lines) - - def to_rerun(self, tree: TfFrameTree | None = None) -> RerunMulti: - """Convert to (entity_path, archetype) pairs to log to rerun. - - Pass a TfFrameTree to also nest a triad per frame under its ancestors, - matching the tf tree's shape in the entity panel. - """ - import rerun as rr - - results: RerunMulti = [] - for transform in self.transforms: - path = f"{DEFAULT_LINKS_ROOT}/{rr.escape_entity_path_part(transform.child_frame_id)}" - results.append((path, transform.to_rerun())) - - if tree is not None: - tree.update(self.transforms) - return results diff --git a/dimos/msgs/trajectory_msgs/JointTrajectory.py b/dimos/msgs/trajectory_msgs/JointTrajectory.py deleted file mode 100644 index ae2ad55fd1b..00000000000 --- a/dimos/msgs/trajectory_msgs/JointTrajectory.py +++ /dev/null @@ -1,211 +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. - -""" -JointTrajectory message type. - -A sequence of joint trajectory points representing a full trajectory. -Similar to ROS trajectory_msgs/JointTrajectory. -""" - -from io import BytesIO -import struct -import time - -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint - - -class JointTrajectory: - """ - A joint-space trajectory consisting of timestamped waypoints. - - Attributes: - timestamp: When trajectory was created (seconds since epoch) - joint_names: Names of joints (optional) - points: Sequence of TrajectoryPoints - duration: Total trajectory duration (seconds) - """ - - msg_name = "trajectory_msgs.JointTrajectory" - - __slots__ = ["duration", "joint_names", "num_joints", "num_points", "points", "timestamp"] - - def __init__( - self, - points: list[TrajectoryPoint] | None = None, - joint_names: list[str] | None = None, - timestamp: float | None = None, - ) -> None: - """ - Initialize JointTrajectory. - - Args: - points: List of TrajectoryPoints - joint_names: Names of joints (optional) - timestamp: Creation timestamp (defaults to now) - """ - self.timestamp = timestamp if timestamp is not None else time.time() - self.points = list(points) if points else [] - self.num_points = len(self.points) - self.joint_names = list(joint_names) if joint_names else [] - self.num_joints = ( - len(self.joint_names) - if self.joint_names - else (self.points[0].num_joints if self.points else 0) - ) - - # Compute duration from last point - if self.points: - self.duration = max(p.time_from_start for p in self.points) - else: - self.duration = 0.0 - - def sample(self, t: float) -> tuple[list[float], list[float]]: - """ - Sample the trajectory at time t using linear interpolation. - - Args: - t: Time from trajectory start (seconds) - - Returns: - Tuple of (positions, velocities) at time t - """ - if not self.points: - return [], [] - - # Clamp t to valid range - t = max(0.0, min(t, self.duration)) - - # Find bracketing points - if t <= self.points[0].time_from_start: - return list(self.points[0].positions), list(self.points[0].velocities) - - if t >= self.points[-1].time_from_start: - return list(self.points[-1].positions), list(self.points[-1].velocities) - - # Find interval - for i in range(len(self.points) - 1): - t0 = self.points[i].time_from_start - t1 = self.points[i + 1].time_from_start - - if t0 <= t <= t1: - # Linear interpolation - alpha = (t - t0) / (t1 - t0) if t1 > t0 else 0.0 - p0 = self.points[i] - p1 = self.points[i + 1] - - positions = [ - p0.positions[j] + alpha * (p1.positions[j] - p0.positions[j]) - for j in range(len(p0.positions)) - ] - velocities = [ - p0.velocities[j] + alpha * (p1.velocities[j] - p0.velocities[j]) - for j in range(len(p0.velocities)) - ] - return positions, velocities - - # Fallback - return list(self.points[-1].positions), list(self.points[-1].velocities) - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(JointTrajectory._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # timestamp (double) - buf.write(struct.pack(">d", self.timestamp)) - # duration (double) - buf.write(struct.pack(">d", self.duration)) - # num_joint_names (int32) - actual count of joint names - buf.write(struct.pack(">i", len(self.joint_names))) - # joint_names (string[num_joint_names]) - for name in self.joint_names: - name_bytes = name.encode("utf-8") - buf.write(struct.pack(">i", len(name_bytes))) - buf.write(name_bytes) - # num_points (int32) - buf.write(struct.pack(">i", self.num_points)) - # points (TrajectoryPoint[num_points]) - for point in self.points: - point._encode_one(buf) - - @classmethod - def lcm_decode(cls, data: bytes) -> "JointTrajectory": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "JointTrajectory": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "JointTrajectory": - self = cls.__new__(cls) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.duration = struct.unpack(">d", buf.read(8))[0] - - # Read joint names - num_joint_names = struct.unpack(">i", buf.read(4))[0] - self.joint_names = [] - for _ in range(num_joint_names): - name_len = struct.unpack(">i", buf.read(4))[0] - self.joint_names.append(buf.read(name_len).decode("utf-8")) - - # Read points - self.num_points = struct.unpack(">i", buf.read(4))[0] - self.points = [TrajectoryPoint._decode_one(buf) for _ in range(self.num_points)] - - # Set num_joints from joint_names or points - self.num_joints = ( - len(self.joint_names) - if self.joint_names - else (self.points[0].num_joints if self.points else 0) - ) - - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x2B3C4D5E6F708192 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"JointTrajectory({self.num_points} points, duration={self.duration:.3f}s)" - - def __repr__(self) -> str: - return ( - f"JointTrajectory(points={self.points}, joint_names={self.joint_names}, " - f"timestamp={self.timestamp})" - ) - - def __len__(self) -> int: - return self.num_points diff --git a/dimos/msgs/trajectory_msgs/TrajectoryPoint.py b/dimos/msgs/trajectory_msgs/TrajectoryPoint.py deleted file mode 100644 index b2b9ab8406c..00000000000 --- a/dimos/msgs/trajectory_msgs/TrajectoryPoint.py +++ /dev/null @@ -1,136 +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. - -""" -TrajectoryPoint message type. - -A single point in a joint trajectory with positions, velocities, and time. -Similar to ROS trajectory_msgs/JointTrajectoryPoint. -""" - -from io import BytesIO -import struct - - -class TrajectoryPoint: - """ - A single point in a joint trajectory. - - Attributes: - time_from_start: Time from trajectory start (seconds) - positions: Joint positions (radians) - velocities: Joint velocities (rad/s) - """ - - msg_name = "trajectory_msgs.TrajectoryPoint" - - __slots__ = ["num_joints", "positions", "time_from_start", "velocities"] - - def __init__( - self, - time_from_start: float = 0.0, - positions: list[float] | None = None, - velocities: list[float] | None = None, - ) -> None: - """ - Initialize TrajectoryPoint. - - Args: - time_from_start: Time from trajectory start (seconds) - positions: Joint positions (radians) - velocities: Joint velocities (rad/s), defaults to zeros if None - """ - self.time_from_start = time_from_start - self.positions = list(positions) if positions else [] - self.num_joints = len(self.positions) - - if velocities is not None: - self.velocities = list(velocities) - else: - self.velocities = [0.0] * self.num_joints - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(TrajectoryPoint._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # time_from_start (double) - buf.write(struct.pack(">d", self.time_from_start)) - # num_joints (int32) - buf.write(struct.pack(">i", self.num_joints)) - # positions (double[num_joints]) - for p in self.positions: - buf.write(struct.pack(">d", p)) - # velocities (double[num_joints]) - for v in self.velocities: - buf.write(struct.pack(">d", v)) - - @classmethod - def lcm_decode(cls, data: bytes) -> "TrajectoryPoint": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "TrajectoryPoint": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "TrajectoryPoint": - self = cls.__new__(cls) - self.time_from_start = struct.unpack(">d", buf.read(8))[0] - self.num_joints = struct.unpack(">i", buf.read(4))[0] - self.positions = [struct.unpack(">d", buf.read(8))[0] for _ in range(self.num_joints)] - self.velocities = [struct.unpack(">d", buf.read(8))[0] for _ in range(self.num_joints)] - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x1A2B3C4D5E6F7081 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"TrajectoryPoint(t={self.time_from_start:.3f}s, {self.num_joints} joints)" - - def __repr__(self) -> str: - return ( - f"TrajectoryPoint(time_from_start={self.time_from_start}, " - f"positions={self.positions}, velocities={self.velocities})" - ) - - def __eq__(self, other) -> bool: # type: ignore[no-untyped-def] - if not isinstance(other, TrajectoryPoint): - return False - return ( - self.time_from_start == other.time_from_start - and self.positions == other.positions - and self.velocities == other.velocities - ) diff --git a/dimos/msgs/trajectory_msgs/TrajectoryStatus.py b/dimos/msgs/trajectory_msgs/TrajectoryStatus.py deleted file mode 100644 index 0a3c117e685..00000000000 --- a/dimos/msgs/trajectory_msgs/TrajectoryStatus.py +++ /dev/null @@ -1,170 +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. - -""" -TrajectoryStatus message type. - -Status feedback for trajectory execution. -""" - -from enum import IntEnum -from io import BytesIO -import struct -import time - - -class TrajectoryState(IntEnum): - """States for trajectory execution.""" - - IDLE = 0 # No trajectory, ready to accept - EXECUTING = 1 # Currently executing trajectory - COMPLETED = 2 # Trajectory finished successfully - ABORTED = 3 # Trajectory was cancelled - FAULT = 4 # Error occurred, requires reset() - - -class TrajectoryStatus: - """ - Status of trajectory execution. - - Attributes: - timestamp: When status was generated - state: Current TrajectoryState - progress: Progress 0.0 to 1.0 - time_elapsed: Seconds since trajectory start - time_remaining: Estimated seconds remaining - error: Error message if FAULT state (empty string otherwise) - """ - - msg_name = "trajectory_msgs.TrajectoryStatus" - - __slots__ = ["error", "progress", "state", "time_elapsed", "time_remaining", "timestamp"] - - def __init__( - self, - state: TrajectoryState = TrajectoryState.IDLE, - progress: float = 0.0, - time_elapsed: float = 0.0, - time_remaining: float = 0.0, - error: str = "", - timestamp: float | None = None, - ) -> None: - """ - Initialize TrajectoryStatus. - - Args: - state: Current execution state - progress: Progress through trajectory (0.0 to 1.0) - time_elapsed: Time since trajectory start (seconds) - time_remaining: Estimated time remaining (seconds) - error: Error message if in FAULT state - timestamp: When status was generated (defaults to now) - """ - self.timestamp = timestamp if timestamp is not None else time.time() - self.state = state - self.progress = progress - self.time_elapsed = time_elapsed - self.time_remaining = time_remaining - self.error = error - - @property - def state_name(self) -> str: - """Get human-readable state name.""" - return self.state.name - - def is_done(self) -> bool: - """Check if trajectory execution is finished (completed, aborted, or fault).""" - return self.state in ( - TrajectoryState.COMPLETED, - TrajectoryState.ABORTED, - TrajectoryState.FAULT, - ) - - def is_active(self) -> bool: - """Check if trajectory is currently executing.""" - return self.state == TrajectoryState.EXECUTING - - def lcm_encode(self) -> bytes: - """Encode for LCM transport.""" - return self.encode() - - def encode(self) -> bytes: - buf = BytesIO() - buf.write(TrajectoryStatus._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf: BytesIO) -> None: - # timestamp (double) - buf.write(struct.pack(">d", self.timestamp)) - # state (int32) - buf.write(struct.pack(">i", int(self.state))) - # progress (double) - buf.write(struct.pack(">d", self.progress)) - # time_elapsed (double) - buf.write(struct.pack(">d", self.time_elapsed)) - # time_remaining (double) - buf.write(struct.pack(">d", self.time_remaining)) - # error (string) - error_bytes = self.error.encode("utf-8") - buf.write(struct.pack(">i", len(error_bytes))) - buf.write(error_bytes) - - @classmethod - def lcm_decode(cls, data: bytes) -> "TrajectoryStatus": - """Decode from LCM transport.""" - return cls.decode(data) - - @classmethod - def decode(cls, data: bytes) -> "TrajectoryStatus": - buf = BytesIO(data) if not hasattr(data, "read") else data - if buf.read(8) != cls._get_packed_fingerprint(): - raise ValueError("Decode error: fingerprint mismatch") - return cls._decode_one(buf) # type: ignore[arg-type] - - @classmethod - def _decode_one(cls, buf: BytesIO) -> "TrajectoryStatus": - self = cls.__new__(cls) - self.timestamp = struct.unpack(">d", buf.read(8))[0] - self.state = TrajectoryState(struct.unpack(">i", buf.read(4))[0]) - self.progress = struct.unpack(">d", buf.read(8))[0] - self.time_elapsed = struct.unpack(">d", buf.read(8))[0] - self.time_remaining = struct.unpack(">d", buf.read(8))[0] - error_len = struct.unpack(">i", buf.read(4))[0] - self.error = buf.read(error_len).decode("utf-8") - return self - - _packed_fingerprint = None - - @classmethod - def _get_hash_recursive(cls, parents): # type: ignore[no-untyped-def] - if cls in parents: - return 0 - return 0x3C4D5E6F708192A3 & 0xFFFFFFFFFFFFFFFF - - @classmethod - def _get_packed_fingerprint(cls) -> bytes: - if cls._packed_fingerprint is None: - cls._packed_fingerprint = struct.pack(">Q", cls._get_hash_recursive([])) # type: ignore[no-untyped-call] - return cls._packed_fingerprint - - def __str__(self) -> str: - return f"TrajectoryStatus({self.state_name}, progress={self.progress:.1%})" - - def __repr__(self) -> str: - return ( - f"TrajectoryStatus(state={self.state_name}, progress={self.progress}, " - f"time_elapsed={self.time_elapsed}, time_remaining={self.time_remaining}, " - f"error='{self.error}')" - ) diff --git a/dimos/msgs/vision_msgs/BoundingBox2DArray.py b/dimos/msgs/vision_msgs/BoundingBox2DArray.py deleted file mode 100644 index f376de6372a..00000000000 --- a/dimos/msgs/vision_msgs/BoundingBox2DArray.py +++ /dev/null @@ -1,21 +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 dimos_lcm.vision_msgs.BoundingBox2DArray import ( - BoundingBox2DArray as LCMBoundingBox2DArray, -) - - -class BoundingBox2DArray(LCMBoundingBox2DArray): # type: ignore[misc] - msg_name = "vision_msgs.BoundingBox2DArray" diff --git a/dimos/msgs/vision_msgs/BoundingBox3DArray.py b/dimos/msgs/vision_msgs/BoundingBox3DArray.py deleted file mode 100644 index d8d7775f919..00000000000 --- a/dimos/msgs/vision_msgs/BoundingBox3DArray.py +++ /dev/null @@ -1,21 +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 dimos_lcm.vision_msgs.BoundingBox3DArray import ( - BoundingBox3DArray as LCMBoundingBox3DArray, -) - - -class BoundingBox3DArray(LCMBoundingBox3DArray): # type: ignore[misc] - msg_name = "vision_msgs.BoundingBox3DArray" diff --git a/dimos/msgs/vision_msgs/Detection2D.py b/dimos/msgs/vision_msgs/Detection2D.py deleted file mode 100644 index aa957f80614..00000000000 --- a/dimos/msgs/vision_msgs/Detection2D.py +++ /dev/null @@ -1,27 +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 dimos_lcm.vision_msgs.Detection2D import Detection2D as LCMDetection2D - -from dimos.types.timestamped import to_timestamp - - -class Detection2D(LCMDetection2D): # type: ignore[misc] - msg_name = "vision_msgs.Detection2D" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection2D.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) diff --git a/dimos/msgs/vision_msgs/Detection2DArray.py b/dimos/msgs/vision_msgs/Detection2DArray.py deleted file mode 100644 index f33cc4cc2ae..00000000000 --- a/dimos/msgs/vision_msgs/Detection2DArray.py +++ /dev/null @@ -1,29 +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 dimos_lcm.vision_msgs.Detection2DArray import ( - Detection2DArray as LCMDetection2DArray, -) - -from dimos.types.timestamped import to_timestamp - - -class Detection2DArray(LCMDetection2DArray): # type: ignore[misc] - msg_name = "vision_msgs.Detection2DArray" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection2DArray.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) diff --git a/dimos/msgs/vision_msgs/Detection3D.py b/dimos/msgs/vision_msgs/Detection3D.py deleted file mode 100644 index 76a838041a4..00000000000 --- a/dimos/msgs/vision_msgs/Detection3D.py +++ /dev/null @@ -1,31 +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 dimos_lcm.vision_msgs.Detection3D import Detection3D as LCMDetection3D - -from dimos.types.timestamped import to_timestamp - - -class Detection3D(LCMDetection3D): # type: ignore[misc] - msg_name = "vision_msgs.Detection3D" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection3D.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) - - @property - def frame_id(self) -> str: - return str(self.header.frame_id) diff --git a/dimos/msgs/vision_msgs/Detection3DArray.py b/dimos/msgs/vision_msgs/Detection3DArray.py deleted file mode 100644 index ffc5da35ee3..00000000000 --- a/dimos/msgs/vision_msgs/Detection3DArray.py +++ /dev/null @@ -1,80 +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 typing import Any - -from dimos_lcm.vision_msgs.Detection3DArray import Detection3DArray as LCMDetection3DArray - -from dimos.types.timestamped import to_timestamp - - -class Detection3DArray(LCMDetection3DArray): # type: ignore[misc] - msg_name = "vision_msgs.Detection3DArray" - - # for _get_field_type() to work when decoding in _decode_one() - __annotations__ = LCMDetection3DArray.__annotations__ - - @property - def ts(self) -> float: - return to_timestamp(self.header.stamp) - - @property - def frame_id(self) -> str: - return str(self.header.frame_id) - - def to_rerun(self) -> Any: - """Convert detections to a Rerun Boxes3D archetype.""" - import rerun as rr - - centers: list[tuple[float, float, float]] = [] - half_sizes: list[tuple[float, float, float]] = [] - quaternions: list[tuple[float, float, float, float]] = [] - labels: list[str] = [] - - for detection in self.detections[: self.detections_length]: - bbox = detection.bbox - center = bbox.center.position - orientation = bbox.center.orientation - size = bbox.size - - centers.append((center.x, center.y, center.z)) - half_sizes.append((size.x / 2.0, size.y / 2.0, size.z / 2.0)) - quaternions.append( - ( - orientation.x, - orientation.y, - orientation.z, - orientation.w, - ) - ) - labels.append(_label_for_detection(detection)) - - return rr.Boxes3D( - centers=centers, - half_sizes=half_sizes, - quaternions=quaternions, - labels=labels, - ) - - -def _label_for_detection(detection: Any) -> str: - marker_id = str(getattr(detection, "id", "")).strip() - for result in detection.results[: detection.results_length]: - class_id = str(result.hypothesis.class_id).strip() - if marker_id and class_id: - return f"{class_id} id={marker_id}" - if class_id: - return class_id - if marker_id: - return f"id={marker_id}" - return "" diff --git a/dimos/msgs/visualization_msgs/EntityMarkers.py b/dimos/msgs/visualization_msgs/EntityMarkers.py deleted file mode 100644 index 86ba74d72e9..00000000000 --- a/dimos/msgs/visualization_msgs/EntityMarkers.py +++ /dev/null @@ -1,145 +0,0 @@ -# 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. - -"""Labeled 3D entity markers for Rerun visualization. - -Published by modules that track entities in world coordinates. -The Rerun bridge picks these up via ``to_rerun()`` and renders them -as labeled colored points overlaid on the 3D scene. -""" - -from __future__ import annotations - -from dataclasses import dataclass -from io import BytesIO -import json -import struct -import time -from typing import TYPE_CHECKING - -if TYPE_CHECKING: - from rerun._baseclasses import Archetype - -from dimos.types.timestamped import Timestamped - -# Entity type → RGBA color -TYPE_COLORS: dict[str, tuple[int, int, int, int]] = { - "person": (255, 100, 100, 255), - "object": (100, 255, 100, 255), - "location": (100, 100, 255, 255), -} -DEFAULT_COLOR = (200, 200, 200, 255) - - -@dataclass -class Marker: - """A single entity marker in world coordinates.""" - - entity_id: str - label: str - entity_type: str # person | object | location - x: float - y: float - z: float - - -class EntityMarkers(Timestamped): - """A batch of labeled 3D entity markers. - - Wire format: JSON-encoded list of markers over LCM string channel. - Rerun: ``rr.Points3D`` with per-point labels and colors. - """ - - msg_name = "visualization_msgs.EntityMarkers" - - def __init__( - self, - markers: list[Marker] | None = None, - ts: float | None = None, - ) -> None: - self.markers: list[Marker] = markers or [] - self.ts: float = ts or time.time() - - # -- LCM serialization (JSON payload) -- - - def _encode_one(self, buf: BytesIO) -> None: - payload = json.dumps( - [ - { - "id": m.entity_id, - "label": m.label, - "type": m.entity_type, - "x": m.x, - "y": m.y, - "z": m.z, - } - for m in self.markers - ] - ).encode() - buf.write(struct.pack(">I", len(payload))) - buf.write(payload) - - def encode(self) -> bytes: - buf = BytesIO() - self._encode_one(buf) - return buf.getvalue() - - @classmethod - def _decode_one(cls, buf: BytesIO) -> EntityMarkers: - (length,) = struct.unpack(">I", buf.read(4)) - payload = json.loads(buf.read(length).decode()) - markers = [ - Marker( - entity_id=m["id"], - label=m["label"], - entity_type=m["type"], - x=m["x"], - y=m["y"], - z=m["z"], - ) - for m in payload - ] - return cls(markers=markers) - - @classmethod - def decode(cls, data: bytes) -> EntityMarkers: - return cls._decode_one(BytesIO(data)) - - # -- Rerun conversion -- - - def to_rerun(self) -> Archetype: - import rerun as rr - - if not self.markers: - return rr.Points3D([]) - - positions = [[m.x, m.y, m.z] for m in self.markers] - labels = [f"{m.entity_id}: {m.label[:40]}" for m in self.markers] - colors = [TYPE_COLORS.get(m.entity_type, DEFAULT_COLOR) for m in self.markers] - - return rr.Points3D( - positions=positions, - labels=labels, - colors=colors, - radii=[0.15] * len(self.markers), - ) - - # -- LCM compat (so autoconnect assigns LCMTransport, not pLCM) -- - - def lcm_encode(self) -> bytes: - return self.encode() - - @classmethod - def lcm_decode(cls, data: bytes, **kwargs: object) -> EntityMarkers: - return cls.decode(data) diff --git a/examples/language-interop/lua/.gitignore b/examples/language-interop/lua/.gitignore deleted file mode 100644 index f87dd2d1251..00000000000 --- a/examples/language-interop/lua/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -lcm/ -dimos-lcm/ -msgs/ diff --git a/examples/language-interop/lua/main.lua b/examples/language-interop/lua/main.lua deleted file mode 100644 index f6d2dccca17..00000000000 --- a/examples/language-interop/lua/main.lua +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env lua - --- Lua robot control example --- Subscribes to robot pose and publishes twist commands - --- Add local msgs folder to path -local script_dir = arg[0]:match("(.*/)") or "./" -package.path = script_dir .. "msgs/?.lua;" .. package.path -package.path = script_dir .. "msgs/?/init.lua;" .. package.path - -local lcm = require("lcm") -local PoseStamped = require("geometry_msgs.PoseStamped") -local Twist = require("geometry_msgs.Twist") -local Vector3 = require("geometry_msgs.Vector3") - -local lc = lcm.lcm.new() - -print("Robot control started") -print("Subscribing to /odom, publishing to /cmd_vel") -print("Press Ctrl+C to stop.\n") - --- Subscribe to pose -lc:subscribe("/odom#geometry_msgs.PoseStamped", function(channel, msg) - msg = PoseStamped.decode(msg) - local pos = msg.pose.position - local ori = msg.pose.orientation - print(string.format("[pose] x=%.2f y=%.2f z=%.2f | qw=%.2f", - pos.x, pos.y, pos.z, ori.w)) -end) - --- Publisher loop -local t = 0 -local socket = require("socket") -local last_pub = socket.gettime() - -while true do - -- Handle incoming messages - lc:handle() - - -- Publish at ~10 Hz - local now = socket.gettime() - if now - last_pub >= 0.1 then - local twist = Twist:new() - twist.linear = Vector3:new() - twist.linear.x = 0.5 - twist.linear.y = 0 - twist.linear.z = 0 - twist.angular = Vector3:new() - twist.angular.x = 0 - twist.angular.y = 0 - twist.angular.z = math.sin(t) * 0.3 - - lc:publish("/cmd_vel#geometry_msgs.Twist", twist:encode()) - print(string.format("[twist] linear=%.2f angular=%.2f", twist.linear.x, twist.angular.z)) - - t = t + 0.1 - last_pub = now - end -end diff --git a/examples/language-interop/lua/setup.sh b/examples/language-interop/lua/setup.sh deleted file mode 100755 index 682d676183e..00000000000 --- a/examples/language-interop/lua/setup.sh +++ /dev/null @@ -1,100 +0,0 @@ -#!/bin/bash -# Setup script for LCM Lua bindings -# Clones official LCM repo and builds Lua bindings -# -# Tested on: Arch Linux, Ubuntu, macOS (with Homebrew) - -set -e - -SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" -LCM_DIR="$SCRIPT_DIR/lcm" - -echo "=== LCM Lua Setup ===" - -# Detect Lua version -if command -v lua &>/dev/null; then - LUA_VERSION=$(lua -v 2>&1 | grep -oE '[0-9]+\.[0-9]+' | head -1) - echo "Detected Lua version: $LUA_VERSION" -else - echo "Error: lua not found in PATH" - exit 1 -fi - -# Detect Lua paths using pkg-config if available -if command -v pkg-config &>/dev/null && pkg-config --exists "lua$LUA_VERSION" 2>/dev/null; then - LUA_INCLUDE_DIR=$(pkg-config --variable=includedir "lua$LUA_VERSION") - LUA_LIBRARY=$(pkg-config --libs "lua$LUA_VERSION" | grep -oE '/[^ ]+\.so' | head -1 || echo "") -elif command -v pkg-config &>/dev/null && pkg-config --exists lua 2>/dev/null; then - LUA_INCLUDE_DIR=$(pkg-config --variable=includedir lua) - LUA_LIBRARY=$(pkg-config --libs lua | grep -oE '/[^ ]+\.so' | head -1 || echo "") -fi - -# Platform-specific defaults -if [[ "$OSTYPE" == "darwin"* ]]; then - # macOS with Homebrew - LUA_INCLUDE_DIR="${LUA_INCLUDE_DIR:-$(brew --prefix lua 2>/dev/null)/include/lua}" - LUA_LIBRARY="${LUA_LIBRARY:-$(brew --prefix lua 2>/dev/null)/lib/liblua.dylib}" - LUA_CPATH_BASE="${LUA_CPATH_BASE:-/usr/local/lib/lua}" -else - # Linux defaults - LUA_INCLUDE_DIR="${LUA_INCLUDE_DIR:-/usr/include}" - LUA_LIBRARY="${LUA_LIBRARY:-/usr/lib/liblua.so}" - LUA_CPATH_BASE="${LUA_CPATH_BASE:-/usr/local/lib/lua}" -fi - -echo "Lua include: $LUA_INCLUDE_DIR" -echo "Lua library: $LUA_LIBRARY" - -# Clone LCM if not present -if [ ! -d "$LCM_DIR" ]; then - echo "Cloning LCM..." - git clone --depth 1 https://github.com/lcm-proj/lcm.git "$LCM_DIR" -else - echo "LCM already cloned" -fi - -# Build Lua bindings using cmake -echo "Building LCM Lua bindings..." -cd "$LCM_DIR" -mkdir -p build && cd build - -# Configure with Lua support -cmake .. \ - -DLCM_ENABLE_LUA=ON \ - -DLCM_ENABLE_PYTHON=OFF \ - -DLCM_ENABLE_JAVA=OFF \ - -DLCM_ENABLE_TESTS=OFF \ - -DLCM_ENABLE_EXAMPLES=OFF \ - -DLUA_INCLUDE_DIR="$LUA_INCLUDE_DIR" \ - -DLUA_LIBRARY="$LUA_LIBRARY" - -# Build just the lua target -make lcm-lua -j$(nproc 2>/dev/null || sysctl -n hw.ncpu 2>/dev/null || echo 4) - -# Install the lua module -LUA_CPATH_DIR="$LUA_CPATH_BASE/$LUA_VERSION" -echo "Installing lcm.so to $LUA_CPATH_DIR" -sudo mkdir -p "$LUA_CPATH_DIR" -sudo cp lcm-lua/lcm.so "$LUA_CPATH_DIR/" - -# Get dimos-lcm message definitions -DIMOS_LCM_DIR="$SCRIPT_DIR/dimos-lcm" -MSGS_DST="$SCRIPT_DIR/msgs" - -echo "Getting message definitions..." -if [ -d "$DIMOS_LCM_DIR" ]; then - echo "Updating dimos-lcm..." - cd "$DIMOS_LCM_DIR" && git pull -else - echo "Cloning dimos-lcm..." - git clone --depth 1 https://github.com/dimensionalOS/dimos-lcm.git "$DIMOS_LCM_DIR" -fi - -# Link/copy messages -rm -rf "$MSGS_DST" -cp -r "$DIMOS_LCM_DIR/generated/lua_lcm_msgs" "$MSGS_DST" -echo "Messages installed to $MSGS_DST" - -echo "" -echo "=== Setup complete ===" -echo "Run: lua main.lua" diff --git a/examples/language-interop/ts/deno.json b/examples/language-interop/ts/deno.json deleted file mode 100644 index b64363a64a7..00000000000 --- a/examples/language-interop/ts/deno.json +++ /dev/null @@ -1,9 +0,0 @@ -{ - "imports": { - "@dimos/lcm": "jsr:@dimos/lcm", - "@dimos/msgs": "jsr:@dimos/msgs" - }, - "tasks": { - "start": "deno run --allow-net --unstable-net main.ts" - } -} diff --git a/examples/language-interop/ts/deno.lock b/examples/language-interop/ts/deno.lock deleted file mode 100644 index 9529ebdb342..00000000000 --- a/examples/language-interop/ts/deno.lock +++ /dev/null @@ -1,21 +0,0 @@ -{ - "version": "5", - "specifiers": { - "jsr:@dimos/lcm@*": "0.2.0", - "jsr:@dimos/msgs@*": "0.1.4" - }, - "jsr": { - "@dimos/lcm@0.2.0": { - "integrity": "03399f5e4800f28a0c294981e0210d784232fc65a57707de19052ad805bd5fea" - }, - "@dimos/msgs@0.1.4": { - "integrity": "564bc30b4bc41a562c296c257a15055283ca0cbd66d0627991ede5295832d0c4" - } - }, - "workspace": { - "dependencies": [ - "jsr:@dimos/lcm@*", - "jsr:@dimos/msgs@*" - ] - } -} diff --git a/examples/language-interop/ts/main.ts b/examples/language-interop/ts/main.ts deleted file mode 100644 index 9026304f2c6..00000000000 --- a/examples/language-interop/ts/main.ts +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env -S deno run --allow-net --unstable-net - -// TypeScript robot control example -// Subscribes to robot odometry and publishes twist commands - -import { LCM } from "@dimos/lcm"; -import { geometry_msgs } from "@dimos/msgs"; - -const lcm = new LCM(); -await lcm.start(); - -console.log("Robot control started"); -console.log("Subscribing to /odom, publishing to /cmd_vel"); -console.log("Press Ctrl+C to stop.\n"); - -// Subscribe to pose - prints robot position -lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { - const pos = msg.data.pose.position; - const ori = msg.data.pose.orientation; - console.log( - `[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)} | qw=${ori.w.toFixed(2)}` - ); -}); - -// Publish twist commands at 10 Hz - simple forward motion -let t = 0; -const interval = setInterval(async () => { - if (!lcm.isRunning()) { - clearInterval(interval); - return; - } - - const twist = new geometry_msgs.Twist({ - linear: new geometry_msgs.Vector3({ x: 0.5, y: 0, z: 0 }), - angular: new geometry_msgs.Vector3({ x: 0, y: 0, z: Math.sin(t) * 0.3 }), - }); - - await lcm.publish("/cmd_vel", twist); - console.log(`[twist] linear=${twist.linear.x.toFixed(2)} angular=${twist.angular.z.toFixed(2)}`); - t += 0.1; -}, 100); - -await lcm.run(); diff --git a/examples/language-interop/ts/web/index.html b/examples/language-interop/ts/web/index.html deleted file mode 100644 index 1ce89604b6b..00000000000 --- a/examples/language-interop/ts/web/index.html +++ /dev/null @@ -1,213 +0,0 @@ - - - - - - - Robot Control - - - -

Robot Control

-
Connecting...
- -
-
-

Pose

-
X--
-
Y--
-
Z--
-
Qw--
-
- -
-

Controls

-
-
- -
- - - -
- -
-
-
- -
-

Log

-
-
- - - - diff --git a/examples/language-interop/ts/web/server.ts b/examples/language-interop/ts/web/server.ts deleted file mode 100644 index 5b8ec035bef..00000000000 --- a/examples/language-interop/ts/web/server.ts +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env -S deno run --allow-net --allow-read --unstable-net - -// LCM to WebSocket Bridge for Robot Control -// Forwards robot pose to browser, receives twist commands from browser - -import { LCM } from "jsr:@dimos/lcm"; -import { decodePacket, geometry_msgs } from "jsr:@dimos/msgs"; - -const PORT = 8080; -const clients = new Set(); - -Deno.serve({ port: PORT }, async (req) => { - const url = new URL(req.url); - - if (req.headers.get("upgrade") === "websocket") { - const { socket, response } = Deno.upgradeWebSocket(req); - socket.onopen = () => { console.log("Client connected"); clients.add(socket); }; - socket.onclose = () => { console.log("Client disconnected"); clients.delete(socket); }; - socket.onerror = () => clients.delete(socket); - - // Forward binary LCM packets from browser directly to UDP - socket.binaryType = "arraybuffer"; - socket.onmessage = async (event) => { - if (event.data instanceof ArrayBuffer) { - const packet = new Uint8Array(event.data); - try { - // we don't need to decode, just showing we can - const { channel, data } = decodePacket(packet); - console.log(`[ws->lcm] ${channel}`, data); - await lcm.publishPacket(packet); - } catch (e) { - console.error("Forward error:", e); - } - } - }; - - return response; - } - - if (url.pathname === "/" || url.pathname === "/index.html") { - const html = await Deno.readTextFile(new URL("./index.html", import.meta.url)); - return new Response(html, { headers: { "content-type": "text/html" } }); - } - - return new Response("Not found", { status: 404 }); -}); - -console.log(`Server: http://localhost:${PORT}`); - -const lcm = new LCM(); -await lcm.start(); - -// Subscribe to pose and just log to show how server can decode messages for itself -lcm.subscribe("/odom", geometry_msgs.PoseStamped, (msg) => { - const pos = msg.data.pose.position; - const ori = msg.data.pose.orientation; - console.log(`[pose] x=${pos.x.toFixed(2)} y=${pos.y.toFixed(2)} z=${pos.z.toFixed(2)}`); -}); - -// Forward all raw packets to browser (we are decoding LCM directly in the browser) -lcm.subscribePacket((packet) => { - for (const client of clients) { - if (client.readyState === WebSocket.OPEN) { - client.send(packet); - } - } -}); - -await lcm.run(); diff --git a/pyproject.toml b/pyproject.toml index 0559e9a603d..8a689056a4e 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -105,7 +105,6 @@ dependencies = [ "dimos-generated==0.1.0", "dimos-message-build==0.1.0", # Transport Protocols - "dimos-lcm>=0.1.3", "lcm-dimos-fork>=1.5.2.post1", # Raw LCM transport; generated messages use CDR. "eclipse-zenoh>=1.10.1,<2.0", "PyTurboJPEG==1.8.2", @@ -522,7 +521,7 @@ dev = [ required-version = ">=0.9.25" default-groups = ["tests"] exclude-newer = "7 days" -exclude-newer-package = { dimos-viewer = false, dimos-lcm = false, lcm-dimos-fork = false, roboplan = false, md-babel-py = false, can-motor-control = false, edgetam-dimos = false, dim-odom = false, eclipse-zenoh = false } +exclude-newer-package = { dimos-viewer = false, lcm-dimos-fork = false, roboplan = false, md-babel-py = false, can-motor-control = false, edgetam-dimos = false, dim-odom = false, eclipse-zenoh = false } constraint-dependencies = [ # TODO(PY311): drop this fork. onnxruntime(-gpu) 1.24 dropped cp310 # wheels, so 3.10 environments resolve 1.23.x (matching what a 3.10 @@ -639,7 +638,6 @@ module = [ "a750_control.*", "bosdyn", "bosdyn.*", - "dimos_lcm.*", "builtin_interfaces.*", "cyclonedds", "cyclonedds.*", diff --git a/uv.lock b/uv.lock index 344d32d3011..e29ace1f334 100644 --- a/uv.lock +++ b/uv.lock @@ -38,7 +38,6 @@ exclude-newer-span = "P7D" [options.exclude-newer-package] can-motor-control = false dim-odom = false -dimos-lcm = false dimos-viewer = false eclipse-zenoh = false edgetam-dimos = false @@ -1718,7 +1717,6 @@ dependencies = [ { name = "cmeel-tinyxml2" }, { name = "cryptography" }, { name = "dimos-generated" }, - { name = "dimos-lcm" }, { name = "dimos-message-build" }, { name = "dimos-viewer" }, { name = "eclipse-zenoh" }, @@ -2367,7 +2365,6 @@ requires-dist = [ { name = "dimos", extras = ["planning", "base", "sim", "cpu"], marker = "extra == 'manipulation'" }, { name = "dimos", extras = ["unitree"], marker = "extra == 'unitree-dds'" }, { name = "dimos-generated", directory = "packages/dimos-generated" }, - { name = "dimos-lcm", specifier = ">=0.1.3" }, { name = "dimos-message-build", directory = "dimos/message_codegen" }, { name = "dimos-viewer", specifier = "==0.32.0a2" }, { name = "dimos-viewer", marker = "extra == 'visualization'", specifier = "==0.32.0a2" }, @@ -2686,21 +2683,6 @@ name = "dimos-generated" version = "0.1.0" source = { directory = "packages/dimos-generated" } -[[package]] -name = "dimos-lcm" -version = "0.1.3" -source = { registry = "https://pypi.org/simple" } -dependencies = [ - { name = "foxglove-websocket" }, - { name = "lcm-dimos-fork" }, - { name = "numpy", version = "2.2.6", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version < '3.11'" }, - { name = "numpy", version = "2.3.5", source = { registry = "https://pypi.org/simple" }, marker = "python_full_version >= '3.11'" }, -] -sdist = { url = "https://files.pythonhosted.org/packages/09/b2/4cdd2bce665ab633313b5d371e0a63fcae2cf77a934a2a9bf820db88a540/dimos_lcm-0.1.3.tar.gz", hash = "sha256:5f7dbd3055f299823bc0e450c59583ad5a2d093c182deec8a86073853881bb09", size = 405688, upload-time = "2026-06-03T07:20:20.552Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/95/19/2d8babf544993508359922ea59db2280d42efa0d5a80d4d0cae22ce40d67/dimos_lcm-0.1.3-py3-none-any.whl", hash = "sha256:63317225a0b4ab0f05e4b656f3f78ac9ba4e914998da2c689ca839cbbd83d57d", size = 1714087, upload-time = "2026-06-03T07:20:22.689Z" }, -] - [[package]] name = "dimos-message-build" version = "0.1.0" @@ -3306,18 +3288,6 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/c7/4e/ce75a57ff3aebf6fc1f4e9d508b8e5810618a33d900ad6c19eb30b290b97/fonttools-4.61.1-py3-none-any.whl", hash = "sha256:17d2bf5d541add43822bcf0c43d7d847b160c9bb01d15d5007d84e2217aaa371", size = 1148996, upload-time = "2025-12-12T17:31:21.03Z" }, ] -[[package]] -name = "foxglove-websocket" -version = "0.1.4" -source = { registry = "https://pypi.org/simple" } -dependencies = [ - { name = "websockets" }, -] -sdist = { url = "https://files.pythonhosted.org/packages/db/b5/df32ac550eb0df9000ed78d872eb19738edecfd88f47fe08588d5066f317/foxglove_websocket-0.1.4.tar.gz", hash = "sha256:2ec8936982e478d103dd90268a572599fc0cce45a4ab95490d5bc31f7c8a8af8", size = 16616, upload-time = "2025-07-14T20:26:28.278Z" } -wheels = [ - { url = "https://files.pythonhosted.org/packages/c4/73/3a3e6cb864ddf98800a9236ad497d32e5b50eb1682ac659f7d669d92faec/foxglove_websocket-0.1.4-py3-none-any.whl", hash = "sha256:772e24e2c98bdfc704df53f7177c8ff5bab0abc4dac59a91463aca16debdd83a", size = 14392, upload-time = "2025-07-14T20:26:26.899Z" }, -] - [[package]] name = "frozenlist" version = "1.8.0"