Ivan/feat/cameracalibrator go2

dimos/mapping/voxels.py

@@ -248,6 +248,7 @@ class VoxelGridMapperConfig(ModuleConfig):
     device: str = "CUDA:0"
     carve_columns: bool = True
     frame_id: str = "world"
+    emit_every: int = 1
 
 
 class VoxelGridMapper(StreamModule[PointCloud2, PointCloud2]):

dimos/perception/fiducial/marker_tf_module.py

@@ -21,10 +21,10 @@
 empty distortion supported best; refine intrinsics on hardware when needed).
 Camera calibration runbook: ``docs/usage/camera_calibration.md``.
 
-The pose chain is ``base_link -> <optical> -> marker`` where ``<optical>`` is
+The pose chain is ``world -> <optical> -> marker`` where ``<optical>`` is
 ``Image.frame_id`` when set, else ``CameraInfo.frame_id``, else ``camera_optical``.
-That matches the frame the pixels live in. Then ``world -> base_link`` is applied
-before publishing the marker namespace.
+That matches the frame the pixels live in. The TF graph resolves ``world -> optical``
+in one lookup; the module no longer needs an intermediate ``base_link`` hop.
 
 OpenCV 4.7+ uses ``ArucoDetector``; pose uses ``solvePnP`` (``estimatePoseSingleMarkers``
 was removed in newer OpenCV builds).
@@ -79,6 +79,13 @@
     from dimos.core.rpc_client import ModuleProxy
 
 
+_FISHEYE_MODELS = frozenset({"equidistant", "fisheye", "kannala_brandt"})
+
+
+def _is_fisheye_model(distortion_model: str | None) -> bool:
+    return (distortion_model or "").strip().lower() in _FISHEYE_MODELS
+
+
 def camera_info_to_cv_matrices(camera_info: CameraInfo) -> tuple[np.ndarray, np.ndarray]:
     """Build OpenCV ``cameraMatrix`` and ``distCoeffs`` from ``CameraInfo``."""
     k = np.array(camera_info.K, dtype=np.float64).reshape(3, 3)
@@ -117,15 +124,34 @@ def estimate_marker_pose(
     marker_length_m: float,
     camera_matrix: np.ndarray,
     dist_coeffs: np.ndarray,
+    *,
+    distortion_model: str | None = None,
 ) -> tuple[np.ndarray, np.ndarray] | None:
-    """Return ``(rvec, tvec)`` for camera optical <- marker from undistorted solvePnP."""
+    """Return ``(rvec, tvec)`` for camera optical <- marker from undistorted solvePnP.
+
+    For fisheye/equidistant intrinsics, corners are first undistorted into the
+    same pinhole ``K`` so the radtan-only ``solvePnP`` sees pinhole-equivalent
+    pixels. Otherwise the radtan ``dist_coeffs`` are passed straight through.
+    """
     obj = _aruco_marker_object_points(marker_length_m)
-    img = corners_px.reshape(4, 1, 2).astype(np.float32)
+    img: np.ndarray = corners_px.reshape(4, 1, 2).astype(np.float32)
+    if _is_fisheye_model(distortion_model):
+        d_flat = np.asarray(dist_coeffs, dtype=np.float64).reshape(-1)
+        if d_flat.size < 4:
+            raise ValueError(
+                f"Fisheye/equidistant distortion model requires at least 4 coefficients; "
+                f"got {d_flat.size}. Check CameraInfo.D."
+            )
+        d_fisheye = d_flat[:4].reshape(4, 1)
+        img = cv2.fisheye.undistortPoints(img, camera_matrix, d_fisheye, P=camera_matrix)
+        solve_dist: np.ndarray = np.zeros((0, 1), dtype=np.float64)
+    else:
+        solve_dist = dist_coeffs
     ok, rvec, tvec = cv2.solvePnP(
         obj,
         img,
         camera_matrix,
-        dist_coeffs,
+        solve_dist,
         flags=cv2.SOLVEPNP_IPPE_SQUARE,
     )
     if not ok:
@@ -170,14 +196,13 @@ class MarkerTfModuleConfig(ModuleConfig):
     """
 
     world_frame: str = "world"
-    base_frame: str = "base_link"
     markers_frame: str = "markers"
     marker_namespace_prefix: str | None = None
     aruco_dictionary: str = "DICT_APRILTAG_36h11"
     marker_length_m: float = Field(
         ..., gt=0.0, description="Physical square marker edge length in meters."
     )
-    max_freq: float = 15.0
+    max_freq: float = 5.0
     tf_lookup_tolerance: float = 0.5
 
 
@@ -207,7 +232,7 @@ def _marker_child_frame(self, marker_id: int) -> str:
 
     def _maybe_warn_distortion(self, camera_info: CameraInfo) -> None:
         model = (camera_info.distortion_model or "").strip().lower()
-        if model in ("", "plumb_bob"):
+        if model in ("", "plumb_bob") or _is_fisheye_model(model):
             return
         if not self._warned_distortion_model:
             logger.warning(
@@ -242,31 +267,16 @@ def _process_color_image(self, image: Image) -> None:
 
         cam_mtx, dist = camera_info_to_cv_matrices(info)
         optical = _camera_optical_frame_id(image, info)
-        t_world_base = self.tf.get(
+        t_world_optical = self.tf.get(
             self.config.world_frame,
-            self.config.base_frame,
-            image.ts,
-            self.config.tf_lookup_tolerance,
-        )
-        if t_world_base is None:
-            logger.debug(
-                "MarkerTfModule: no TF %s -> %s at ts=%s",
-                self.config.world_frame,
-                self.config.base_frame,
-                image.ts,
-            )
-            return
-
-        t_base_optical = self.tf.get(
-            self.config.base_frame,
             optical,
             image.ts,
             self.config.tf_lookup_tolerance,
         )
-        if t_base_optical is None:
+        if t_world_optical is None:
             logger.debug(
                 "MarkerTfModule: no TF %s -> %s at ts=%s",
-                self.config.base_frame,
+                self.config.world_frame,
                 optical,
                 image.ts,
             )
@@ -291,6 +301,7 @@ def _process_color_image(self, image: Image) -> None:
                 self.config.marker_length_m,
                 cam_mtx,
                 dist,
+                distortion_model=info.distortion_model,
             )
             if pose is None:
                 continue
@@ -302,8 +313,7 @@ def _process_color_image(self, image: Image) -> None:
                 child_frame_id="__marker_tmp",
                 ts=ts,
             )
-            t_base_marker = t_base_optical + t_optical_marker
-            t_world_marker = t_world_base + t_base_marker
+            t_world_marker = t_world_optical + t_optical_marker
             out.append(
                 Transform(
                     translation=t_world_marker.translation,

dimos/protocol/pubsub/registry.py

@@ -0,0 +1,171 @@
+# 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.
+
+"""Pubsub URI registry: ``"<proto>:<topic>[#<msg_type>]"`` -> started ``PubSubTransport``.
+
+Maps user-facing protocol names onto the concrete transport classes in
+``dimos.core.transport``. Used by CLIs and config files that need to accept
+a single string describing both how and where to subscribe.
+
+URI grammar::
+
+    <proto>:<topic>[#<msg_type>]
+
+- ``<proto>``: registry key, e.g. ``lcm``, ``jpeg_lcm``, ``plcm``, ``pshm``,
+  ``shm``, ``jpeg_shm``.
+- ``<topic>``: channel/key, passed verbatim to the transport constructor.
+- ``<msg_type>``: optional ``module.ClassName`` resolved via
+  ``dimos.msgs.helpers.resolve_msg_type`` (e.g. ``sensor_msgs.Image``).
+
+Typed protos (``lcm``, ``jpeg_lcm``) require a message type — either from the
+``#``-suffix or the ``msg_type`` kwarg. Pickled / self-describing protos
+(``plcm``, ``pshm``, ``shm``, ``jpeg_shm``) don't.
+"""
+
+from __future__ import annotations
+
+from typing import TYPE_CHECKING, Any
+
+if TYPE_CHECKING:
+    from collections.abc import Callable
+
+    from dimos.core.transport import PubSubTransport
+
+
+def _make_lcm(topic: str, msg_type: type | None) -> Any:
+    if msg_type is None:
+        raise ValueError("proto 'lcm' requires a message type (URI '#suffix' or msg_type kwarg)")
+    from dimos.core.transport import LCMTransport
+
+    return LCMTransport(topic, msg_type)
+
+
+def _make_jpeg_lcm(topic: str, msg_type: type | None) -> Any:
+    if msg_type is None:
+        raise ValueError(
+            "proto 'jpeg_lcm' requires a message type (URI '#suffix' or msg_type kwarg)"
+        )
+    from dimos.core.transport import JpegLcmTransport
+
+    return JpegLcmTransport(topic, msg_type)
+
+
+def _make_plcm(topic: str, msg_type: type | None) -> Any:
+    # pickled LCM: receivers unpickle Python objects, no type registration needed.
+    from dimos.core.transport import pLCMTransport
+
+    return pLCMTransport(topic)
+
+
+def _make_pshm(topic: str, msg_type: type | None) -> Any:
+    # pickled shared memory: same shape as plcm but over /dev/shm.
+    from dimos.core.transport import pSHMTransport
+
+    return pSHMTransport(topic)
+
+
+def _make_shm(topic: str, msg_type: type | None) -> Any:
+    # raw-bytes shared memory: subscribers receive bytes; caller decodes.
+    from dimos.core.transport import SHMTransport
+
+    return SHMTransport(topic)
+
+
+def _make_jpeg_shm(topic: str, msg_type: type | None) -> Any:
+    # JPEG-encoded shared memory: subscribers receive decoded Image objects.
+    from dimos.core.transport import JpegShmTransport
+
+    return JpegShmTransport(topic)
+
+
+_REGISTRY: dict[str, Callable[[str, type | None], Any]] = {
+    "lcm": _make_lcm,
+    "jpeg_lcm": _make_jpeg_lcm,
+    "plcm": _make_plcm,
+    "pshm": _make_pshm,
+    "shm": _make_shm,
+    "jpeg_shm": _make_jpeg_shm,
+}
+
+
+def supported_protos() -> list[str]:
+    """Return the sorted list of registered proto names."""
+    return sorted(_REGISTRY.keys())
+
+
+def parse_pubsub_uri(uri: str) -> tuple[str, str, str | None]:
+    """Split ``"<proto>:<topic>[#<msg_type>]"`` into its three parts.
+
+    Returns ``(proto, topic, msg_type_name_or_None)``. Raises ``ValueError``
+    on malformed input or an unknown proto.
+    """
+    if ":" not in uri:
+        raise ValueError(
+            f"Invalid pubsub URI {uri!r}: expected '<proto>:<topic>'. "
+            f"Supported protos: {supported_protos()}"
+        )
+    proto, rest = uri.split(":", 1)
+    if not proto:
+        raise ValueError(f"Invalid pubsub URI {uri!r}: empty proto")
+    if proto not in _REGISTRY:
+        raise ValueError(f"Unsupported proto {proto!r}; supported: {supported_protos()}")
+    msg_type_name: str | None
+    if "#" in rest:
+        topic, suffix = rest.split("#", 1)
+        msg_type_name = suffix or None
+    else:
+        topic, msg_type_name = rest, None
+    if not topic:
+        raise ValueError(f"Invalid pubsub URI {uri!r}: empty topic")
+    return proto, topic, msg_type_name
+
+
+def make_pubsub_transport(
+    uri: str,
+    *,
+    msg_type: type | None = None,
+) -> PubSubTransport[Any]:
+    """Build a ``PubSubTransport`` from a URI (does not call ``start()``).
+
+    The ``#``-suffix in the URI wins over the ``msg_type`` kwarg if both are
+    present. Pickled / self-describing protos ignore ``msg_type``.
+    """
+    proto, topic, msg_type_name = parse_pubsub_uri(uri)
+    resolved = msg_type
+    if msg_type_name is not None:
+        from dimos.msgs.helpers import resolve_msg_type
+
+        resolved = resolve_msg_type(msg_type_name)
+        if resolved is None:
+            raise ValueError(f"Could not resolve message type {msg_type_name!r} from URI {uri!r}")
+    transport: PubSubTransport[Any] = _REGISTRY[proto](topic, resolved)
+    return transport
+
+
+def subscribe_pubsub_uri(
+    uri: str,
+    callback: Callable[[Any], Any],
+    *,
+    msg_type: type | None = None,
+) -> tuple[PubSubTransport[Any], Callable[[], None]]:
+    """Construct + start + subscribe in one step.
+
+    Returns ``(transport, unsubscribe)``. The caller is responsible for
+    calling ``transport.stop()`` (and ``unsubscribe()`` if it needs to stop
+    receiving messages before the transport itself stops).
+    """
+    transport = make_pubsub_transport(uri, msg_type=msg_type)
+    transport.start()
+    unsub = transport.subscribe(callback)
+    return transport, unsub