diff --git a/dimos/constants.py b/dimos/constants.py index d849f4aaf3..2f71d10c14 100644 --- a/dimos/constants.py +++ b/dimos/constants.py @@ -53,3 +53,6 @@ DEFAULT_THREAD_JOIN_TIMEOUT = 2.0 DEFAULT_BUILD_NATIVE = False + +DEFAULT_WORLD_FRAME = "world" +DEFAULT_ROBOT_FRAME = "base_link" diff --git a/dimos/core/module.py b/dimos/core/module.py index 26a2b6f893..847d335de1 100644 --- a/dimos/core/module.py +++ b/dimos/core/module.py @@ -109,6 +109,11 @@ class ModuleConfig(BaseConfig): tf_transport: type[TFSpec] = LCMTF # type: ignore[type-arg] frame_id_prefix: str | None = None frame_id: str | None = None + # TODO: later expose frame remappings, somehow, at the blueprint level + # frame mapping is how others can remap frame names, in the future we should have a way to do this remapping at the blueprint level too + # (static-transform publishing lives on the TfModule subclass, but frame_mapping + # stays here because blueprint-level frame namespacing applies to every module) + frame_mapping: dict[str, str] = Field(default_factory=dict) g: GlobalConfig = global_config @@ -149,6 +154,7 @@ def __init__(self, config_args: dict[str, Any]) -> None: self._tools = {} self._tools_lock = threading.Lock() self._loop, self._loop_thread = get_loop() + self.frame_mapping = self._setup_frame_mapping() try: self.rpc = self.config.rpc_transport( # type: ignore[call-arg] rpc_timeouts=self.config.rpc_timeouts, @@ -191,6 +197,34 @@ def stop(self) -> None: super().stop() self._close_module() + def _setup_frame_mapping(self) -> dict[str, str]: + # frame_mapping resolution stays on the base Module (every module exposes a + # resolved frame_mapping for blueprint-level namespacing). Static-transform + # publishing lives on the TfModule subclass. + frame_mapping_field = type(self.config).model_fields["frame_mapping"] + if not hasattr(frame_mapping_field, "default_factory") or not callable( + frame_mapping_field.default_factory + ): + raise ValueError( + f"""In the {self.name!r} module config definition, the frame_mapping needs to be a pydantic field, not a dict""" + ) + # given something like: + # class MyConfig: + # frame_mapping: dict[str, str] = Field(default_factory=lambda: dict( + # body="base_link", + # parent="world", + # )) + # the "body" is what I call a common_name + # "base_link" is the REAL frame id that other modules can query/use + existing_frames: dict[str, str] = frame_mapping_field.default_factory() # type: ignore[call-arg] + final_frame_mapping = {**existing_frames, **self.config.frame_mapping} + for existing_frame, remapped_frame in final_frame_mapping.items(): + if existing_frame not in existing_frames: + raise ValueError( + f"""On module {self.name}, tried to map {existing_frame!r} to {remapped_frame!r} but that first frame doesn't exist. The existing ones are: {list(existing_frames.keys())!r} """ + ) + return final_frame_mapping + def _close_module(self) -> None: with self._module_closed_lock: if self._module_closed: diff --git a/dimos/core/tf_module.py b/dimos/core/tf_module.py new file mode 100644 index 0000000000..f0955464a1 --- /dev/null +++ b/dimos/core/tf_module.py @@ -0,0 +1,142 @@ +# 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. +import threading +import time +from typing import Any + +from pydantic import Field + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.msgs.geometry_msgs.Transform import Transform + + +class TfModuleConfig(ModuleConfig): + static_transforms: dict[str, Transform] = Field(default_factory=dict) + # TODO: in the future we should make self.tf.publish error if it tried to publish a transform that references a frame that is not mentioned in this dict (same with self.tf.get) + static_publish_interval: float = 1.0 + + +class TfModule(Module): + """A Module that republishes its config's static (non-moving) transforms on an interval. + + Modules that need to publish fixed frames inherit from this instead of Module + directly, so the base Module stays free of transform-publishing machinery that + most modules don't need. `frame_mapping` resolution still lives on Module (every + module needs it for blueprint-level frame namespacing); only the static-transform + publishing is added here. + """ + + config: TfModuleConfig + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._static_publish_thread: threading.Thread | None = None + # created lazily in start() so the module stays picklable for worker deployment + self._static_publish_stop: threading.Event | None = None + self.static_transforms = self._resolve_static_transforms() + + @rpc + def start(self) -> None: + # NOTE: there's basically always going to be some inital race around static transform frames and tf.get's + # publishing the statics before main starts helps mitigate/reduce that + self._start_static_publish() + super().start() + + @rpc + def stop(self) -> None: + self._stop_static_publish() + super().stop() + + def _resolve_static_transforms(self) -> dict[str, Transform]: + frame_mapping_field = type(self.config).model_fields["frame_mapping"] + existing_frames: dict[str, str] = frame_mapping_field.default_factory() # type: ignore[misc,union-attr,call-arg] + + # step1 translate urdf_name=>common_name (see Module._setup_frame_mapping for what + # "common name" vs "real frame id" means) + reverse_mapping = {value: key for key, value in existing_frames.items()} + static_transforms_common_names = { + reverse_mapping.get(urdf_frame_id, urdf_frame_id): Transform( + translation=transform.translation, + rotation=transform.rotation, + frame_id=reverse_mapping.get(transform.frame_id, transform.frame_id), + child_frame_id=reverse_mapping.get( + transform.child_frame_id, transform.child_frame_id + ), + ) + for urdf_frame_id, transform in self.config.static_transforms.items() + } + # step2 map common_name=>real_frame_id using the module's resolved frame_mapping + final_frame_mapping = self.frame_mapping + return { + final_frame_mapping.get(common_frame_id, common_frame_id): Transform( + translation=transform.translation, + rotation=transform.rotation, + frame_id=final_frame_mapping.get(transform.frame_id, transform.frame_id), + child_frame_id=final_frame_mapping.get( + transform.child_frame_id, transform.child_frame_id + ), + ) + for common_frame_id, transform in static_transforms_common_names.items() + } + + def _start_static_publish(self) -> None: + self._static_publish() + if not self.static_transforms or self.config.static_publish_interval <= 0: + return + self._static_publish_stop = threading.Event() + self._static_publish_thread = threading.Thread( + target=self._static_publisher, + daemon=True, + ) + self._static_publish_thread.start() + + # TODO: later this should be replaced with latching streams + def _static_publisher(self) -> None: + stop = self._static_publish_stop + assert stop is not None + while not stop.wait(self.config.static_publish_interval): + self._static_publish() + + def _static_publish(self) -> None: + if not self.static_transforms: + return + now = time.time() + self.tf.publish_static( + *( + Transform( + translation=transform.translation, + rotation=transform.rotation, + frame_id=transform.frame_id, + child_frame_id=transform.child_frame_id, + ts=now, + ) + for transform in self.static_transforms.values() + ) + ) + self._on_static_publish() + + def _on_static_publish(self) -> None: + """ + This is a callback for modules to publish other data (ex: camera info) in the static loop + This should be rarely used, but exists for the few cases where it is needed + """ + + def _stop_static_publish(self) -> None: + if self._static_publish_stop is not None: + self._static_publish_stop.set() + if self._static_publish_thread and self._static_publish_thread.is_alive(): + self._static_publish_thread.join(timeout=self._loop_thread_timeout) + self._static_publish_thread = None + self._static_publish_stop = None diff --git a/dimos/hardware/sensors/lidar/pointlio/config/no_gravity_align.yaml b/dimos/hardware/sensors/lidar/pointlio/config/no_gravity_align.yaml new file mode 100644 index 0000000000..ed9c057083 --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/config/no_gravity_align.yaml @@ -0,0 +1,62 @@ +# default.yaml with gravity self-leveling turned off (mapping.gravity_align). +# Used when the mount tilt is corrected by a known static transform instead of +# letting Point-LIO level the odometry to gravity. Keep the rest in sync with +# default.yaml. +common: + con_frame: false + con_frame_num: 1 + cut_frame: false + cut_frame_time_interval: 0.1 + time_lag_imu_to_lidar: 0.0 + +preprocess: + lidar_type: 1 + scan_line: 4 + scan_rate: 10 + timestamp_unit: 3 # 3 = nanosecond + blind: 0.5 + point_filter_num: 3 + +mapping: + use_imu_as_input: false + prop_at_freq_of_imu: true + check_satu: true + init_map_size: 10 + space_down_sample: true + satu_acc: 3.0 + satu_gyro: 35.0 + acc_norm: 1.0 + plane_thr: 0.1 + filter_size_surf: 0.2 + filter_size_map: 0.5 + ivox_grid_resolution: 2.0 + ivox_nearby_type: 6 + cube_side_length: 1000.0 + det_range: 100.0 + fov_degree: 360.0 + imu_en: true + start_in_aggressive_motion: false + extrinsic_est_en: false + imu_time_inte: 0.005 + lidar_meas_cov: 0.01 + acc_cov_input: 0.1 + vel_cov: 20.0 + gyr_cov_input: 0.01 + gyr_cov_output: 1000.0 + acc_cov_output: 500.0 + b_gyr_cov: 0.0001 + b_acc_cov: 0.0001 + imu_meas_acc_cov: 0.01 + imu_meas_omg_cov: 0.01 + match_s: 81.0 + gravity_align: false + gravity: [0.0, 0.0, -9.810] + gravity_init: [0.0, 0.0, -9.810] + extrinsic_T: [-0.011, -0.02329, 0.04412] # Mid-360 IMU->lidar offset (m) + extrinsic_R: [1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0] + +odometry: + publish_odometry_without_downsample: false + odom_only: false diff --git a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp index 9c7bcb3714..d2aef4a4cf 100644 --- a/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp +++ b/dimos/hardware/sensors/lidar/pointlio/cpp/main.cpp @@ -16,6 +16,8 @@ #include +#include + #include #include #include @@ -81,6 +83,17 @@ static std::string g_child_frame_id; // required via --child_frame_id static std::string g_sensor_frame_id; // required via --sensor_frame_id static float g_frequency = 10.0f; +// Optional rigid transform applied to the published cloud + odometry (and, via +// the Python TF republisher that reads the odometry, the odom->body TF). Lets a +// physically-tilted mount be reported as a different mount without touching the +// SLAM core. Identity unless --transform supplies a row-major 4x4 (16 doubles). +// Cloud points are moved p' = C @ p; the body pose is conjugated T' = C @ T @ +// inv(C) and the twist rotated by C's rotation, so cloud and odometry stay +// mutually consistent. +static bool g_has_transform = false; +static Eigen::Matrix4d g_transform = Eigen::Matrix4d::Identity(); +static Eigen::Matrix4d g_transform_inv = Eigen::Matrix4d::Identity(); + // Frame accumulator (Livox SDK raw → CustomMsg) static std::mutex g_pc_mutex; // Serializes all Point-LIO EKF access. The SDK delivers IMU on its own callback @@ -142,11 +155,22 @@ static void publish_lidar(PointCloudXYZI::Ptr cloud, double timestamp, const std pc.data_length = pc.row_step; pc.data.resize(pc.data_length); + const Eigen::Matrix3d transform_rotation = g_transform.block<3, 3>(0, 0); + const Eigen::Vector3d transform_translation = g_transform.block<3, 1>(0, 3); for (int point_idx = 0; point_idx < num_points; ++point_idx) { float* dst = reinterpret_cast(pc.data.data() + point_idx * 16); - dst[0] = cloud->points[point_idx].x; - dst[1] = cloud->points[point_idx].y; - dst[2] = cloud->points[point_idx].z; + double x = cloud->points[point_idx].x; + double y = cloud->points[point_idx].y; + double z = cloud->points[point_idx].z; + if (g_has_transform) { + const Eigen::Vector3d moved = transform_rotation * Eigen::Vector3d(x, y, z) + transform_translation; + x = moved.x(); + y = moved.y(); + z = moved.z(); + } + dst[0] = static_cast(x); + dst[1] = static_cast(y); + dst[2] = static_cast(z); dst[3] = cloud->points[point_idx].intensity; } @@ -161,25 +185,48 @@ static void publish_odometry(const custom_messages::Odometry& odom, double times msg.child_frame_id = g_child_frame_id; // Pose in the SLAM/sensor frame. - msg.pose.pose.position.x = odom.pose.pose.position.x; - msg.pose.pose.position.y = odom.pose.pose.position.y; - msg.pose.pose.position.z = odom.pose.pose.position.z; - msg.pose.pose.orientation.x = odom.pose.pose.orientation.x; - msg.pose.pose.orientation.y = odom.pose.pose.orientation.y; - msg.pose.pose.orientation.z = odom.pose.pose.orientation.z; - msg.pose.pose.orientation.w = odom.pose.pose.orientation.w; + Eigen::Vector3d position( + odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z); + Eigen::Quaterniond orientation( + odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, + odom.pose.pose.orientation.y, odom.pose.pose.orientation.z); + Eigen::Vector3d linear( + odom.twist.twist.linear.x, odom.twist.twist.linear.y, odom.twist.twist.linear.z); + Eigen::Vector3d angular( + odom.twist.twist.angular.x, odom.twist.twist.angular.y, odom.twist.twist.angular.z); + + if (g_has_transform) { + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + pose.block<3, 3>(0, 0) = orientation.toRotationMatrix(); + pose.block<3, 1>(0, 3) = position; + const Eigen::Matrix4d faked = g_transform * pose * g_transform_inv; + const Eigen::Matrix3d transform_rotation = g_transform.block<3, 3>(0, 0); + position = faked.block<3, 1>(0, 3); + orientation = Eigen::Quaterniond(faked.block<3, 3>(0, 0)); + orientation.normalize(); + linear = transform_rotation * linear; + angular = transform_rotation * angular; + } + + msg.pose.pose.position.x = position.x(); + msg.pose.pose.position.y = position.y(); + msg.pose.pose.position.z = position.z(); + msg.pose.pose.orientation.x = orientation.x(); + msg.pose.pose.orientation.y = orientation.y(); + msg.pose.pose.orientation.z = orientation.z(); + msg.pose.pose.orientation.w = orientation.w(); for (int idx = 0; idx < 36; ++idx) { msg.pose.covariance[idx] = odom.pose.covariance[idx]; } // Velocity from Point-LIO's IESKF state (its key output over FAST-LIO). - msg.twist.twist.linear.x = odom.twist.twist.linear.x; - msg.twist.twist.linear.y = odom.twist.twist.linear.y; - msg.twist.twist.linear.z = odom.twist.twist.linear.z; - msg.twist.twist.angular.x = odom.twist.twist.angular.x; - msg.twist.twist.angular.y = odom.twist.twist.angular.y; - msg.twist.twist.angular.z = odom.twist.twist.angular.z; + msg.twist.twist.linear.x = linear.x(); + msg.twist.twist.linear.y = linear.y(); + msg.twist.twist.linear.z = linear.z(); + msg.twist.twist.angular.x = angular.x(); + msg.twist.twist.angular.y = angular.y(); + msg.twist.twist.angular.z = angular.z(); std::memset(msg.twist.covariance, 0, sizeof(msg.twist.covariance)); g_lcm->publish(g_odometry_topic, &msg); @@ -368,6 +415,16 @@ int main(int argc, char** argv) { if (auto gi = parse_doubles(mod.arg("gravity_init", "")); !gi.empty()) params.gravity_init = gi; if (auto et = parse_doubles(mod.arg("extrinsic_t", "")); !et.empty()) params.extrinsic_T = et; if (auto er = parse_doubles(mod.arg("extrinsic_r", "")); !er.empty()) params.extrinsic_R = er; + // Optional post-SLAM mount correction (row-major 4x4); identity when absent. + if (auto transform = parse_doubles(mod.arg("transform", "")); transform.size() == 16) { + g_has_transform = true; + for (int row = 0; row < 4; ++row) { + for (int col = 0; col < 4; ++col) { + g_transform(row, col) = transform[row * 4 + col]; + } + } + g_transform_inv = g_transform.inverse(); + } // odometry params.publish_odometry_without_downsample = mod.arg_bool("publish_odometry_without_downsample", params.publish_odometry_without_downsample); diff --git a/dimos/hardware/sensors/lidar/pointlio/module.py b/dimos/hardware/sensors/lidar/pointlio/module.py index 9db92e9f6d..4b95470d11 100644 --- a/dimos/hardware/sensors/lidar/pointlio/module.py +++ b/dimos/hardware/sensors/lidar/pointlio/module.py @@ -34,7 +34,7 @@ import os from typing import TYPE_CHECKING, Literal -from pydantic import Field +from pydantic import Field, field_validator from reactivex.disposable import Disposable from dimos.core.core import rpc @@ -58,7 +58,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_ODOM +from dimos.navigation.cmu_nav.frames import FRAME_BODY, FRAME_ODOM, FRAME_SENSOR from dimos.spec import perception # Human-readable enums; the C++ binary (main.cpp) maps these strings to @@ -76,18 +76,37 @@ class PointLioConfig(NativeModuleConfig): executable: str = "result/bin/pointlio_native" build_command: str | None = "nix build .#pointlio_native" # lidar_ip required; host_ip optional (auto-derived from lidar_ip's subnet). - # Both fall back to DIMOS_POINTLIO_LIDAR_IP / DIMOS_POINTLIO_HOST_IP. + # Fall back to DIMOS_MID360_LIDAR_IP / DIMOS_POINTLIO_HOST_IP. host_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_HOST_IP")) - lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_POINTLIO_LIDAR_IP")) + lidar_ip: str | None = Field(default_factory=lambda: os.environ.get("DIMOS_MID360_LIDAR_IP")) frequency: float = 10.0 - # Odometry is published as frame_id (fixed) -> child_frame_id (moving body), - # and also broadcast on TF. The point cloud is stamped with sensor_frame_id - # (the lidar's own frame — get_body_cloud is the undistorted scan, not yet - # transformed into the body frame). - frame_id: str = FRAME_ODOM - child_frame_id: str = FRAME_BODY - sensor_frame_id: str = "mid360_link" + # Optional rigid mount correction applied to the published cloud, odometry, + # and TF before they leave the binary: row-major 4x4 (16 floats), None = + # identity. The cloud is moved p' = C @ p; the body pose is conjugated + # T' = C @ T @ inv(C) and the twist rotated by C's rotation, so the cloud and + # odometry stay mutually consistent and a forward walk stays forward. + transform: list[float] | None = None + + @field_validator("transform") + @classmethod + def _validate_transform(cls, value: list[float] | None) -> list[float] | None: + if value is not None and len(value) != 16: + raise ValueError(f"transform must be a row-major 4x4 (16 floats), got {len(value)}") + return value + + # Common-name -> real-frame map. Odometry is published as odom (fixed) -> + # body (moving) and broadcast on TF; the cloud is stamped with the sensor + # frame. A blueprint can rename any of these (e.g. send body to a dead leaf + # when a downstream shim republishes the real odom->body) without touching + # this code. These names are also handed to the C++ binary as CLI args. + frame_mapping: dict[str, str] = Field( + default_factory=lambda: { + FRAME_ODOM: FRAME_ODOM, + FRAME_BODY: FRAME_BODY, + FRAME_SENSOR: "mid360_link", + } + ) # Point-LIO internal processing rates (Hz) msr_freq: float = 50.0 @@ -177,6 +196,18 @@ class PointLio(NativeModule, perception.Lidar, perception.Odometry): @rpc def start(self) -> None: self._validate_network() + # The C++ binary stamps its cloud/odometry/TF with these frames. They live + # in frame_mapping (so a blueprint can rename any of them) and are not + # auto-emitted by to_cli_args, so hand them over explicitly. + self.config.extra_args = [ + *self.config.extra_args, + "--frame_id", + self.frame_mapping[FRAME_ODOM], + "--child_frame_id", + self.frame_mapping[FRAME_BODY], + "--sensor_frame_id", + self.frame_mapping[FRAME_SENSOR], + ] super().start() self.register_disposable( Disposable(self.odometry.transport.subscribe(self._on_odom_for_tf, self.odometry)) @@ -185,8 +216,8 @@ def start(self) -> None: def _on_odom_for_tf(self, msg: Odometry) -> None: self.tf.publish( Transform( - frame_id=self.frame_id, - child_frame_id=self.config.child_frame_id, + frame_id=self.frame_mapping[FRAME_ODOM], + child_frame_id=self.frame_mapping[FRAME_BODY], translation=Vector3( msg.pose.position.x, msg.pose.position.y, @@ -213,7 +244,7 @@ def _validate_network(self) -> None: if not lidar_ip: raise RuntimeError( "PointLio: lidar_ip not set — it's network-specific. Set it in the config " - "or via the DIMOS_POINTLIO_LIDAR_IP env var." + "or via the DIMOS_MID360_LIDAR_IP env var." ) # host_ip optional: derive the local NIC on lidar_ip's /24 when unset or # not one of our IPs (shared with the Mid360 driver). diff --git a/dimos/hardware/sensors/lidar/pointlio/mount_correction.py b/dimos/hardware/sensors/lidar/pointlio/mount_correction.py new file mode 100644 index 0000000000..6f99398faf --- /dev/null +++ b/dimos/hardware/sensors/lidar/pointlio/mount_correction.py @@ -0,0 +1,81 @@ +# 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. + +"""Compute the rigid transform that makes a physically-tilted sensor look like a +differently-mounted one. + +``mount_correction_matrix(original_static_tf, new_static_tf)`` returns the +row-major 4x4 (flattened to 16 floats) to hand to ``PointLio``'s ``transform`` +arg. PointLio then moves every cloud point ``p' = C @ p`` and conjugates the body +pose ``T' = C @ T @ inv(C)`` before publishing, so the whole stack downstream sees +a sensor mounted per ``new_static_tf`` even though it is physically mounted per +``original_static_tf``. Usage:: + + PointLio.blueprint( + transform=mount_correction_matrix(rotated_urdf, normal_urdf), + config="no_gravity_align.yaml", + ) + +``C = inv(new_base_to_sensor) @ original_base_to_sensor`` at the shared +``sensor_frame``. +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.robot.urdf_loader import UrdfLoader + + +def _transform_to_matrix(transform: Transform) -> np.ndarray: + matrix = np.eye(4) + matrix[:3, :3] = transform.rotation.to_rotation_matrix() + matrix[:3, 3] = ( + transform.translation.x, + transform.translation.y, + transform.translation.z, + ) + return matrix + + +def _base_to_frame_matrix(loader: UrdfLoader, leaf_frame: str) -> np.ndarray: + """Compose the fixed-joint chain from the model root down to ``leaf_frame``.""" + static_transforms = loader.static_transforms + chain: list[Transform] = [] + frame = leaf_frame + while frame in static_transforms: + transform = static_transforms[frame] + chain.append(transform) + frame = transform.frame_id + matrix = np.eye(4) + for transform in reversed(chain): + matrix = matrix @ _transform_to_matrix(transform) + return matrix + + +def mount_correction_matrix( + original_static_tf: Path, + new_static_tf: Path, + sensor_frame: str = "mid360_link", +) -> list[float]: + """Row-major 4x4 (16 floats) mapping the ``original`` mount to the ``new`` one.""" + original = _base_to_frame_matrix( + UrdfLoader(name="original", model_path=original_static_tf), sensor_frame + ) + new = _base_to_frame_matrix(UrdfLoader(name="new", model_path=new_static_tf), sensor_frame) + correction = np.linalg.inv(new) @ original + return [float(value) for value in correction.flatten()] diff --git a/dimos/mapping/loop_closure/eval.py b/dimos/mapping/loop_closure/eval.py index ddd9d4cb2a..04ea228359 100644 --- a/dimos/mapping/loop_closure/eval.py +++ b/dimos/mapping/loop_closure/eval.py @@ -41,7 +41,7 @@ from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.sensor_msgs.Image import Image from dimos.perception.fiducial.marker_transformer import DetectMarkers -from dimos.robot.unitree.go2.connection import _camera_info_static +from dimos.robot.unitree.go2.config import camera_info_static from dimos.utils.data import get_data DEFAULT_DATASETS = [f"hk_village{i}" for i in range(1, 7)] @@ -68,7 +68,7 @@ def _eval_recording( ) -> tuple[float, float]: """Returns (pgo_time_s, spread_m) for one recording.""" db_path = get_data(f"{name}.db") - cam_info = _camera_info_static() + cam_info = camera_info_static() store = SqliteStore(path=str(db_path)) with store: diff --git a/dimos/mapping/loop_closure/utils/markers_rrd.py b/dimos/mapping/loop_closure/utils/markers_rrd.py index a98e79ddc3..09d124089f 100644 --- a/dimos/mapping/loop_closure/utils/markers_rrd.py +++ b/dimos/mapping/loop_closure/utils/markers_rrd.py @@ -40,7 +40,7 @@ from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.fiducial.marker_transformer import DetectMarkers -from dimos.robot.unitree.go2.connection import _camera_info_static +from dimos.robot.unitree.go2.config import camera_info_static from dimos.utils.data import resolve_named_path TIMELINE = "ts" @@ -64,7 +64,7 @@ def main( ), ) -> None: db_path = resolve_named_path(dataset, ".db") - cam_info = _camera_info_static() + cam_info = camera_info_static() rr.init("dimos markers", recording_id=db_path.stem) rr.save(str(out)) diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py index cae3e04114..e611bf3c92 100644 --- a/dimos/mapping/utils/cli/map.py +++ b/dimos/mapping/utils/cli/map.py @@ -395,7 +395,7 @@ def main( from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.fiducial.marker_transformer import DetectMarkers - from dimos.robot.unitree.go2.connection import BASE_TO_OPTICAL, _camera_info_static + from dimos.robot.unitree.go2.config import Go2Config, camera_info_static from dimos.utils.data import resolve_named_path from dimos.visualization.rerun.init import rerun_init @@ -516,8 +516,12 @@ def main( store.stream(image_pose).from_time(seek or None).to_time(duration) ) print(f"re-posing color_image from {image_pose!r} + camera optical mount") - color_image = pose_fill(color_image, src_pose, tolerance=0.1, mount=BASE_TO_OPTICAL) - cam_info = CameraInfo.from_yaml(str(camera_info)) if camera_info else _camera_info_static() + base_to_optical = ( + Go2Config.static_transforms["camera_link"] + + Go2Config.static_transforms["camera_optical"] + ) + color_image = pose_fill(color_image, src_pose, tolerance=0.1, mount=base_to_optical) + cam_info = CameraInfo.from_yaml(str(camera_info)) if camera_info else camera_info_static() xf = DetectMarkers( camera_info=cam_info, marker_length_m=marker_size, diff --git a/dimos/mapping/utils/cli/replay.py b/dimos/mapping/utils/cli/replay.py index c78704f725..22af2800a4 100644 --- a/dimos/mapping/utils/cli/replay.py +++ b/dimos/mapping/utils/cli/replay.py @@ -214,13 +214,13 @@ def main( from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation - from dimos.robot.unitree.go2.connection import _camera_info_static + from dimos.robot.unitree.go2.config import camera_info_static from dimos.utils.data import resolve_named_path db_path = resolve_named_path(dataset, ".db") if out is None: out = Path.cwd() / f"{db_path.stem}.rrd" - cam_info = _camera_info_static() + cam_info = camera_info_static() # Resolve which streams to voxelize: all PointCloud2 streams, or the # explicit --map-source subset. Validate up front so typos fail fast. diff --git a/dimos/mapping/utils/cli/replay_marker.py b/dimos/mapping/utils/cli/replay_marker.py index 4d04815051..07f97cb5a2 100644 --- a/dimos/mapping/utils/cli/replay_marker.py +++ b/dimos/mapping/utils/cli/replay_marker.py @@ -72,13 +72,13 @@ def main( from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.fiducial.marker_transformer import DetectMarkers - from dimos.robot.unitree.go2.connection import _camera_info_static + from dimos.robot.unitree.go2.config import camera_info_static from dimos.utils.data import resolve_named_path db_path = resolve_named_path(dataset, ".db") if out is None: out = Path.cwd() / f"{db_path.stem}.rrd" - cam_info = _camera_info_static() + cam_info = camera_info_static() rr.init("dimos markers", recording_id=db_path.stem) rr.save(str(out)) diff --git a/dimos/memory2/module.py b/dimos/memory2/module.py index e16c1527e7..c2c3161b6b 100644 --- a/dimos/memory2/module.py +++ b/dimos/memory2/module.py @@ -27,7 +27,7 @@ from reactivex.disposable import Disposable from dimos.agents.annotation import skill -from dimos.constants import DIMOS_PROJECT_ROOT +from dimos.constants import DEFAULT_ROBOT_FRAME, DEFAULT_WORLD_FRAME, DIMOS_PROJECT_ROOT from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.memory2.embed import EmbedImages @@ -261,9 +261,10 @@ class OnExisting(str, enum.Enum): class RecorderConfig(MemoryModuleConfig): on_existing: OnExisting = OnExisting.BACKUP backup_keep_last: int = Field(default=10, ge=0) - root_frame: str = "world" - default_frame_id: str = "base_link" + root_frame: str = DEFAULT_WORLD_FRAME + default_frame_id: str = DEFAULT_ROBOT_FRAME tf_tolerance: float = 0.5 + tf_warning_interval: float = 5.0 db_path: str | Path = "recording.db" # Also record the live tf stream (under "tf") alongside the In ports. record_tf: bool = True diff --git a/dimos/memory2/utils/db_to_rrd.py b/dimos/memory2/utils/db_to_rrd.py new file mode 100644 index 0000000000..5845a6250d --- /dev/null +++ b/dimos/memory2/utils/db_to_rrd.py @@ -0,0 +1,155 @@ +# 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. + +"""Render a memory2 recording (.db) into rerun. + +Loads the TF tree if the recording has one, then logs every PointCloud2 and +Odometry stream. Clouds are drawn in their stored coordinates (Point-LIO's cloud +is already registered into its world frame); each odom stream gets a moving pose +frame plus its full path as a trail. ``--seconds`` bounds the window from the +start of the recording. +""" + +from __future__ import annotations + +from pathlib import Path +import shutil +import subprocess + +import rerun as rr +import typer + +from dimos.memory2.store.sqlite import SqliteStore +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2, register_colormap_annotation +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.utils.data import resolve_named_path +from dimos.visualization.rerun.init import rerun_init + +TIMELINE = "time" + + +def _open_viewer(rrd: str) -> None: + exe = shutil.which("rerun") + if exe: + subprocess.Popen([exe, rrd]) + print(f"opening {rrd} in rerun") + else: + print(f"rerun viewer not found on PATH; open manually:\n rerun {rrd}") + + +def _classify(store: SqliteStore) -> tuple[list[str], list[str], list[str], float | None]: + """Sort streams into (clouds, odoms, tfs) by their first observation's type, + and return the earliest timestamp across them (the timeline anchor).""" + clouds: list[str] = [] + odoms: list[str] = [] + tfs: list[str] = [] + t0: float | None = None + for name in store.list_streams(): + try: + first = store.streams[name].first() + except LookupError: + continue + data = first.data + if isinstance(data, PointCloud2): + clouds.append(name) + elif isinstance(data, Odometry): + odoms.append(name) + elif isinstance(data, TFMessage): + tfs.append(name) + else: + continue + t0 = first.ts if t0 is None else min(t0, first.ts) + return clouds, odoms, tfs, t0 + + +def main( + dataset: str = typer.Argument(..., help="Recording .db: bare name (cwd or data/) or path"), + out: Path | None = typer.Option( + None, "--out", help="Output .rrd path (default: next to the .db)" + ), + seconds: float | None = typer.Option( + None, "--seconds", help="Only render the first N seconds of the recording" + ), + no_gui: bool = typer.Option(False, "--no-gui", help="Write the .rrd but don't open the viewer"), +) -> None: + db_path = resolve_named_path(dataset, ".db") + store = SqliteStore(path=str(db_path), must_exist=True) + with store: + clouds, odoms, tfs, t0 = _classify(store) + if t0 is None: + print("no PointCloud2 / Odometry / TF streams found in this recording") + raise typer.Exit(1) + print(f"clouds={clouds}") + print(f"odoms={odoms}") + print(f"tf={tfs or '(none)'}") + + rerun_init("db_to_rrd") + rrd_path = str(out) if out is not None else str(db_path.with_suffix(".rrd")) + rr.save(rrd_path) + register_colormap_annotation("turbo") + + def in_window(ts: float) -> bool: + return seconds is None or ts - t0 <= seconds + + # TF first so the tf graph exists before framed clouds reference it. + for name in tfs: + for obs in store.streams[name]: + if not in_window(obs.ts): + break + if obs.data is None: + continue + rr.set_time(TIMELINE, duration=obs.ts - t0) + for path, archetype in obs.data.to_rerun(): + rr.log(path, archetype) + + # Clouds are logged in their stored coordinates (Point-LIO's cloud is + # already registered into its world frame). Each stream is its own entity + # so you can toggle them independently in the viewer. + for name in clouds: + entity = f"world/clouds/{name}" + for obs in store.streams[name]: + if not in_window(obs.ts): + break + cloud = obs.data + if cloud is None: + continue + rr.set_time(TIMELINE, duration=obs.ts - t0) + rr.log(entity, cloud.to_rerun(mode="points")) + + for name in odoms: + trail: list[list[float]] = [] + for obs in store.streams[name]: + if not in_window(obs.ts): + break + odom = obs.data + if odom is None: + continue + rr.set_time(TIMELINE, duration=obs.ts - t0) + # Moving pose frame (has its own transform)... + rr.log(f"world/poses/{name}", odom.to_rerun()) + trail.append([odom.x, odom.y, odom.z]) + # ...and the full path as a static line in world coords (kept off the + # posed entity so it isn't transformed by the pose). + if trail: + rr.log(f"world/trails/{name}", rr.LineStrips3D([trail]), static=True) + + rr.rerun_shutdown() + print(f"wrote {rrd_path}") + if not no_gui: + _open_viewer(rrd_path) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/memory2/vis/space/rerun.py b/dimos/memory2/vis/space/rerun.py index 1929ecef71..59e69ce0ed 100644 --- a/dimos/memory2/vis/space/rerun.py +++ b/dimos/memory2/vis/space/rerun.py @@ -22,11 +22,10 @@ from dimos.memory2.type.observation import Observation from dimos.memory2.vis.color import Color from dimos.memory2.vis.space.elements import Arrow, Box3D, Camera, Point, Polyline, Pose, Text -from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.robot.unitree.go2.config import Go2Config if TYPE_CHECKING: from dimos.memory2.vis.space.space import Space @@ -39,20 +38,6 @@ def _rgba(el: Any) -> tuple[int, int, int, int]: return c.with_alpha(c.a * opacity).rgba_u8() -# base_link → camera_optical extrinsics (applied at render time for image observations) -_BASE_TO_OPTICAL = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", -) + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", -) - - def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: """Render a Space to a Rerun viewer.""" import rerun as rr @@ -243,8 +228,12 @@ def render(space: Space, app_id: str = "space", spawn: bool = True) -> None: continue img = _as_image(data) if img is not None: - # Apply base→optical extrinsics for camera frustum rendering - world_T_optical = Transform.from_pose("world", ps) + _BASE_TO_OPTICAL + # TODO: fix there should be a standard way to set camera frames + base_to_optical = ( + Go2Config.static_transforms["camera_link"] + + Go2Config.static_transforms["camera_optical"] + ) + world_T_optical = Transform.from_pose("world", ps) + base_to_optical rr.log(path, world_T_optical.to_pose().to_rerun(), static=True) h, w = img.shape[:2] focal = max(w, h) diff --git a/dimos/perception/detection/conftest.py b/dimos/perception/detection/conftest.py index aa326804c0..1672847985 100644 --- a/dimos/perception/detection/conftest.py +++ b/dimos/perception/detection/conftest.py @@ -34,7 +34,7 @@ from dimos.perception.detection.type.detection3d.imageDetections3DPC import ImageDetections3DPC from dimos.perception.detection.type.detection3d.pointcloud import Detection3DPC from dimos.protocol.tf.tf import TF -from dimos.robot.unitree.go2 import connection +from dimos.robot.unitree.go2.config import camera_info_static, odom_to_tf from dimos.robot.unitree.type.odometry import Odometry from dimos.utils.data import get_data from dimos.utils.testing.legacy_pickle import LegacyPickleStore @@ -97,7 +97,7 @@ def moment_provider(**kwargs) -> Moment: if odom_frame is None: raise ValueError("No odom frame found") - transforms = connection.GO2Connection._odom_to_tf(odom_frame) + transforms = odom_to_tf(odom_frame) tf.receive_transform(*transforms) @@ -105,7 +105,7 @@ def moment_provider(**kwargs) -> Moment: "odom_frame": odom_frame, "lidar_frame": lidar_frame, "image_frame": image_frame, - "camera_info": connection._camera_info_static(), + "camera_info": camera_info_static(), "transforms": transforms, "tf": tf, } @@ -246,8 +246,8 @@ def object_db_module(get_moment): c = mock.create_autospec(CameraInfo, spec_set=True, instance=True) module2d = Detection2DModule(detector=lambda: Yolo2DDetector(device="cpu"), camera_info=c) - module3d = Detection3DModule(camera_info=connection._camera_info_static()) - moduleDB = ObjectDBModule(camera_info=connection._camera_info_static()) + module3d = Detection3DModule(camera_info=camera_info_static()) + moduleDB = ObjectDBModule(camera_info=camera_info_static()) # Process 5 frames to build up object history for i in range(5): diff --git a/dimos/protocol/tf/test_tf.py b/dimos/protocol/tf/test_tf.py index 281e99c0d7..e27631d1e3 100644 --- a/dimos/protocol/tf/test_tf.py +++ b/dimos/protocol/tf/test_tf.py @@ -18,8 +18,10 @@ import threading import time +from pydantic import Field import pytest +from dimos.core.tf_module import TfModule, TfModuleConfig from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -805,3 +807,156 @@ def test_get_with_longer_transform_chain(self) -> None: assert result.frame_id == "world" assert result.child_frame_id == "hand" + + +class TestStaticTransforms: + LCM_PROPAGATION_DELAY = 0.2 + MODULE_START_DELAY = 0.05 + STRICT_TOLERANCE = 0.001 + STALE_AGE = 1000 + FAR_FUTURE_OFFSET = 9999 + + def test_static_survives_time_tolerance(self) -> None: + """Static transforms should be retrievable regardless of time tolerance.""" + buffer = MultiTBuffer(buffer_size=10.0) + old_time = time.time() - self.STALE_AGE + + camera_link = Transform( + translation=Vector3(0.3, 0.0, 0.1), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ts=old_time, + ) + buffer.receive_static_transform(camera_link) + + result = buffer.get_transform( + "base_link", + "camera_link", + time_point=time.time(), + time_tolerance=self.STRICT_TOLERANCE, + ) + assert result is not None + assert abs(result.translation.x - 0.3) < 1e-6 + + def test_static_not_pruned_by_dynamic(self) -> None: + """Adding dynamic transforms to a different pair shouldn't prune statics from buffers.""" + buffer = MultiTBuffer(buffer_size=2.0) + + camera_link = Transform( + translation=Vector3(0.3, 0.0, 0.1), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ts=time.time() - self.STALE_AGE, + ) + buffer.receive_static_transform(camera_link) + + for i in range(20): + buffer.receive_transform( + Transform( + translation=Vector3(float(i), 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="world", + child_frame_id="base_link", + ts=time.time(), + ) + ) + + assert ("base_link", "camera_link") in buffer.buffers + assert len(buffer.buffers[("base_link", "camera_link")]) > 0 + + def test_static_in_bfs_chain(self) -> None: + """BFS should find paths through static frames.""" + buffer = MultiTBuffer(buffer_size=10.0) + now = time.time() + + buffer.receive_transform( + Transform( + translation=Vector3(1.0, 0.0, 0.0), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="world", + child_frame_id="base_link", + ts=now, + ) + ) + buffer.receive_static_transform( + Transform( + translation=Vector3(0.3, 0.0, 0.1), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ts=now, + ) + ) + + result = buffer.get("world", "camera_link") + assert result is not None + assert abs(result.translation.x - 1.3) < 1e-6 + assert abs(result.translation.z - 0.1) < 1e-6 + + def test_static_publish_over_lcm(self) -> None: + """Static transforms published on /tf_static should be received as statics.""" + broadcaster = TF() + receiver = TF() + + camera_link = Transform( + translation=Vector3(0.3, 0.0, 0.1), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ts=time.time(), + ) + broadcaster.publish_static(camera_link) + time.sleep(self.LCM_PROPAGATION_DELAY) + + assert ("base_link", "camera_link") in receiver._statics + + result = receiver.get( + "base_link", + "camera_link", + time_point=time.time() + self.FAR_FUTURE_OFFSET, + time_tolerance=self.STRICT_TOLERANCE, + ) + assert result is not None + assert abs(result.translation.x - 0.3) < 1e-6 + + broadcaster.stop() + receiver.stop() + + def test_module_publishes_static_transforms(self) -> None: + """A TfModule with static_transforms config should publish them via tf.""" + + class TestConfig(TfModuleConfig): + frame_mapping: dict[str, str] = Field( + default_factory=lambda: dict( + body="base_link", + parent="world", + camera_link="camera_link", + ) + ) + static_transforms: dict[str, Transform] = Field( + default_factory=lambda: { + "camera_link": Transform( + translation=Vector3(0.3, 0.0, 0.1), + rotation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="base_link", + child_frame_id="camera_link", + ), + } + ) + + class TestModule(TfModule): + config: TestConfig + + module = TestModule() + module.start() + time.sleep(self.MODULE_START_DELAY) + + result = module.tf.get("base_link", "camera_link", time_tolerance=self.STRICT_TOLERANCE) + assert result is not None + assert abs(result.translation.x - 0.3) < 1e-6 + assert abs(result.translation.z - 0.1) < 1e-6 + + module.stop() + module._close_module() diff --git a/dimos/protocol/tf/tf.py b/dimos/protocol/tf/tf.py index eb3d72b470..f641a68ed9 100644 --- a/dimos/protocol/tf/tf.py +++ b/dimos/protocol/tf/tf.py @@ -115,6 +115,7 @@ class MultiTBuffer: def __init__(self, buffer_size: float = 10.0) -> None: self.buffers: dict[tuple[str, str], TBuffer] = {} self.buffer_size = buffer_size + self._statics: dict[tuple[str, str], Transform] = {} self._cv = threading.Condition() def receive_transform(self, *args: Transform) -> None: @@ -124,8 +125,32 @@ def receive_transform(self, *args: Transform) -> None: if key not in self.buffers: self.buffers[key] = TBuffer(self.buffer_size) self.buffers[key].add(transform) + # pruning happens in self.buffers[key].add(), but we don't want static ones to get pruned + # would be better to fix pruning somehow but the class that does the pruning + # is more generic than TF-speficic pruning, so I'm going to backfill instead for now + # plz improve when the latching-topic change is made + self._backfill_statics() self._cv.notify_all() + def receive_static_transform(self, *args: Transform) -> None: + with self._cv: + for transform in args: + key = (transform.frame_id, transform.child_frame_id) + if key not in self.buffers: + self.buffers[key] = TBuffer(self.buffer_size) + self.buffers[key].save(transform) + self._statics[key] = transform + self._cv.notify_all() + + def _backfill_statics(self) -> None: + """Re-add static transforms to buffers if pruning removed them.""" + for key, transform in self._statics.items(): + buffer = self.buffers.get(key) + if buffer is None or len(buffer) == 0: + if buffer is None: + self.buffers[key] = TBuffer(self.buffer_size) + self.buffers[key].save(transform) + def get_frames(self) -> set[str]: frames = set() with self._cv: @@ -160,13 +185,18 @@ def get_transform( ) with self._cv: - # Check forward direction key = (parent_frame, child_frame) + reverse_key = (child_frame, parent_frame) + + # Check statics first (no tolerance needed) + if key in self._statics: + return self._statics[key] + if reverse_key in self._statics: + return self._statics[reverse_key].inverse() + + # Check dynamic buffers if key in self.buffers: return self.buffers[key].get(time_point, time_tolerance) # type: ignore[arg-type] - - # Check reverse direction and return inverse - reverse_key = (child_frame, parent_frame) if reverse_key in self.buffers: transform = self.buffers[reverse_key].get(time_point, time_tolerance) # type: ignore[arg-type] return transform.inverse() if transform else None @@ -274,6 +304,46 @@ def get_transform_search( return None + @property + def tree_str(self) -> str: + with self._cv: + keys = list(self.buffers.keys()) + + if not keys: + return "(empty)" + + children: dict[str, list[str]] = {} + all_children: set[str] = set() + for parent, child in keys: + children.setdefault(parent, []).append(child) + all_children.add(child) + + roots = [frame for frame in children if frame not in all_children] + if not roots: + roots = sorted(children.keys())[:1] + + for root in roots: + children.setdefault(root, []) + for frame_list in children.values(): + frame_list.sort() + + lines: list[str] = [] + + def walk(frame: str, prefix: str, is_last: bool, is_root: bool) -> None: + connector = "" if is_root else ("└── " if is_last else "├── ") + lines.append(f"{prefix}{connector}{frame}") + child_prefix = prefix if is_root else prefix + (" " if is_last else "│ ") + kids = children.get(frame, []) + for index, kid in enumerate(kids): + walk(kid, child_prefix, index == len(kids) - 1, False) + + for index, root in enumerate(sorted(roots)): + if index > 0: + lines.append("") + walk(root, "", index == len(roots) - 1, True) + + return "\n".join(lines) + def graph(self) -> str: import subprocess @@ -312,6 +382,7 @@ def __str__(self) -> str: class PubSubTFConfig(TFConfig): topic: Topic | None = None # Required field but needs default for dataclass inheritance + static_topic: Topic | None = None pubsub: type[PubSub] | PubSub | None = None # type: ignore[type-arg] autostart: bool = True @@ -341,6 +412,9 @@ def start(self, sub: bool = True) -> None: topic = getattr(self.config, "topic", None) if topic: self.pubsub.subscribe(topic, self.receive_msg) + static_topic = getattr(self.config, "static_topic", None) + if static_topic: + self.pubsub.subscribe(static_topic, self._receive_static_msg) def stop(self) -> None: self.pubsub.stop() @@ -356,20 +430,28 @@ def publish(self, *args: Transform) -> None: self.pubsub.publish(topic, TFMessage(*args)) def publish_static(self, *args: Transform) -> None: - raise NotImplementedError("Static transforms not implemented in PubSubTF.") + self.receive_static_transform(*args) + static_topic = getattr(self.config, "static_topic", None) + if static_topic: + self.pubsub.publish(static_topic, TFMessage(*args)) def publish_all(self) -> None: """Publish all transforms currently stored in all buffers.""" - all_transforms = [] + dynamic = [] + static = [] with self._cv: - for buffer in self.buffers.values(): - # Get the latest transform from each buffer + for key, buffer in self.buffers.items(): latest = buffer.get() # get() with no args returns latest if latest: - all_transforms.append(latest) + if key in self._statics: + static.append(latest) + else: + dynamic.append(latest) - if all_transforms: - self.publish(*all_transforms) + if dynamic: + self.publish(*dynamic) + if static: + self.publish_static(*static) def get( self, @@ -411,9 +493,13 @@ def get_pose( def receive_msg(self, msg: TFMessage, topic: Topic) -> None: self.receive_tfmessage(msg) + def _receive_static_msg(self, msg: TFMessage, topic: Topic) -> None: + self.receive_static_transform(*msg.transforms) + class LCMPubsubConfig(PubSubTFConfig): topic: Topic = field(default_factory=lambda: Topic("/tf", TFMessage)) + static_topic: Topic = field(default_factory=lambda: Topic("/tf_static", TFMessage)) pubsub: type[PubSub] | PubSub | None = LCM # type: ignore[type-arg] autostart: bool = True diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 61220a0fae..44cefcf859 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -121,6 +121,7 @@ "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory", "unitree-go2-mid360-record": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record:unitree_go2_mid360_record", "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d", + "unitree-go2-nav-3d-rotated": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d_rotated", "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security", diff --git a/dimos/robot/assembly/mid360_realsense_30.py b/dimos/robot/assembly/mid360_realsense_30.py index bc884a04a5..f0415687c2 100644 --- a/dimos/robot/assembly/mid360_realsense_30.py +++ b/dimos/robot/assembly/mid360_realsense_30.py @@ -25,9 +25,9 @@ captures a raw .pcap of the Mid-360 UDP stream). The lidar IPs come from each module's own config (``DIMOS_MID360_LIDAR_IP`` for the -Mid-360 / pcap capture, ``DIMOS_POINTLIO_LIDAR_IP`` for Point-LIO):: +Mid-360 / pcap capture and Point-LIO both read ``DIMOS_MID360_LIDAR_IP``):: - export DIMOS_MID360_LIDAR_IP=192.168.1.155 DIMOS_POINTLIO_LIDAR_IP=192.168.1.155 + export DIMOS_MID360_LIDAR_IP=192.168.1.155 dimos run mid360-realsense-record # db only dimos run mid360-realsense-record-with-pcap # db + raw pcap @@ -58,6 +58,7 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.navigation.cmu_nav.frames import FRAME_ODOM from dimos.protocol.tf.static_tf_publisher import ( FrameSpec, StaticTfPublisher, @@ -157,7 +158,7 @@ class Mid360RealsenseRecorder(PointlioRecorder): (Mid360, "imu", "livox_imu"), ] ), - PointLio.blueprint(frame_id="world").remappings( + PointLio.blueprint(frame_mapping={FRAME_ODOM: "world"}).remappings( [ (PointLio, "lidar", "pointlio_lidar"), (PointLio, "odometry", "pointlio_odometry"), diff --git a/dimos/robot/assembly/mid360_realsense_30.urdf b/dimos/robot/assembly/mid360_realsense_30.urdf new file mode 100644 index 0000000000..a132b2930f --- /dev/null +++ b/dimos/robot/assembly/mid360_realsense_30.urdf @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/diy/alfred/blueprints/alfred_nav.py b/dimos/robot/diy/alfred/blueprints/alfred_nav.py index 16530ffadf..69669f11b6 100644 --- a/dimos/robot/diy/alfred/blueprints/alfred_nav.py +++ b/dimos/robot/diy/alfred/blueprints/alfred_nav.py @@ -51,6 +51,7 @@ alfred_nav = ( autoconnect( + # FIXME: need alfred static transform publisher FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.1.5"), lidar_ip=os.getenv("LIDAR_IP", "192.168.1.189"), diff --git a/dimos/robot/diy/alfred/config.py b/dimos/robot/diy/alfred/config.py index 163e14ad4b..8425a8e072 100644 --- a/dimos/robot/diy/alfred/config.py +++ b/dimos/robot/diy/alfred/config.py @@ -14,11 +14,10 @@ from __future__ import annotations -from dataclasses import dataclass, field -from typing import Any +from dataclasses import dataclass from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.robot.unitree.g1.config import G1_LOCAL_PLANNER_PRECOMPUTED_PATHS @@ -35,15 +34,27 @@ class AlfredConfig: name: str height_clearance: float width_clearance: float - internal_odom_offsets: dict[str, Any] = field(default_factory=dict) + # Lidar mount pose relative to base (used as the LIO init pose). Alfred has no + # URDF, so the static transform below is defined manually from this mount. + sensor_mount: Pose + + @property + def static_transforms(self) -> dict[str, Transform]: + mount = self.sensor_mount + return { + "mid360_link": Transform( + translation=Vector3(mount.x, mount.y, mount.z), + rotation=mount.orientation, + frame_id="base_link", + child_frame_id="mid360_link", + ), + } ALFRED = AlfredConfig( name="alfred", height_clearance=2.0, # meters width_clearance=1.0, - internal_odom_offsets={ - # Mid-360 lidar: a bit forward, and a bit to the right of base center, above ground. - "mid360_link": Pose(0.20, -0.20, 0.30, *Quaternion.from_euler(Vector3(0, 0, 0))), - }, + # Mid-360 lidar: a bit forward, and a bit to the right of base center, above ground. + sensor_mount=Pose(0.20, -0.20, 0.30), ) diff --git a/dimos/robot/model_parser.py b/dimos/robot/model_parser.py index 7040d98fc3..43cc68c102 100644 --- a/dimos/robot/model_parser.py +++ b/dimos/robot/model_parser.py @@ -37,6 +37,9 @@ class JointDescription: effort_limit: float | None = None parent_link: str = "" child_link: str = "" + # Joint origin in the parent link's frame. Defaults to identity. + origin_xyz: tuple[float, float, float] = (0.0, 0.0, 0.0) + origin_rpy: tuple[float, float, float] = (0.0, 0.0, 0.0) @dataclass @@ -139,6 +142,13 @@ def _parse_urdf_string(xml_string: str) -> ModelDescription: velocity = _float_or_none(limit_elem.get("velocity")) effort = _float_or_none(limit_elem.get("effort")) + origin_xyz = (0.0, 0.0, 0.0) + origin_rpy = (0.0, 0.0, 0.0) + origin_elem = joint_elem.find("origin") + if origin_elem is not None: + origin_xyz = _parse_triple(origin_elem.get("xyz", "0 0 0")) + origin_rpy = _parse_triple(origin_elem.get("rpy", "0 0 0")) + joints.append( JointDescription( name=name, @@ -149,6 +159,8 @@ def _parse_urdf_string(xml_string: str) -> ModelDescription: effort_limit=effort, parent_link=parent_link, child_link=child_link, + origin_xyz=origin_xyz, + origin_rpy=origin_rpy, ) ) @@ -237,3 +249,10 @@ def _float_or_none(value: str | None) -> float | None: return float(value) except ValueError: return None + + +def _parse_triple(value: str) -> tuple[float, float, float]: + parts = value.split() + if len(parts) != 3: + raise ValueError(f"Expected 3 floats, got {value!r}") + return (float(parts[0]), float(parts[1]), float(parts[2])) diff --git a/dimos/robot/unitree/dimsim_connection.py b/dimos/robot/unitree/dimsim_connection.py index f40ad53409..d613a3f596 100644 --- a/dimos/robot/unitree/dimsim_connection.py +++ b/dimos/robot/unitree/dimsim_connection.py @@ -21,14 +21,12 @@ from dimos.core.global_config import GlobalConfig from dimos.core.transport import LCMTransport from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.tf.tf import LCMTF +from dimos.robot.unitree.go2.config import odom_to_tf from dimos.simulation.dimsim.dimsim_process import DimSimProcess from dimos.utils.logging_config import setup_logger @@ -105,41 +103,4 @@ def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: return {} def _handle_odom(self, msg: PoseStamped) -> None: - self._tf.publish(*_odom_to_tf(msg)) - - -def _odom_to_tf(odom: PoseStamped) -> list[Transform]: - """Build transform chain from odometry pose. - - Transform tree: world -> base_link -> {camera_link -> camera_optical, lidar_link} - """ - camera_link = Transform( - translation=Vector3(0.3, 0.0, 0.0), # camera 30cm forward - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=odom.ts, - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=odom.ts, - ) - - lidar_link = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="lidar_link", - ts=odom.ts, - ) - - return [ - Transform.from_pose("base_link", odom), - camera_link, - camera_optical, - lidar_link, - ] + self._tf.publish(*odom_to_tf(msg)) diff --git a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py index e3ccbe6a36..2e0a730988 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/unitree_g1_onboard.py @@ -24,10 +24,11 @@ # Underscore-prefixed: a shared sub-blueprint, not a runnable blueprint of its own. _unitree_g1_onboard = autoconnect( + # FIXME: need g1 static transform publisher FastLio2.blueprint( host_ip=os.getenv("LIDAR_HOST_IP", "192.168.123.164"), lidar_ip=os.getenv("LIDAR_IP", "192.168.123.120"), - ), + ).remappings([(FastLio2, "global_map", "global_map_fastlio")]), G1HighLevelDdsSdk.blueprint(), unitree_g1_vis, ).global_config(n_workers=12, robot_model="unitree_g1") diff --git a/dimos/robot/unitree/g1/config.py b/dimos/robot/unitree/g1/config.py index 24e85466be..e6c8b08a80 100644 --- a/dimos/robot/unitree/g1/config.py +++ b/dimos/robot/unitree/g1/config.py @@ -12,17 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""G1 physical description and sensor odometry offsets.""" +"""G1 physical description, URDF frames, and sensor mounting.""" from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass from pathlib import Path -from typing import Any -from dimos.msgs.geometry_msgs.Pose import Pose -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.robot.urdf_loader import UrdfLoader from dimos.utils.data import LfsPath # this is robot-specific, but only needed for the local_planner module @@ -36,19 +34,22 @@ class G1Config: """Physical metadata used by G1 navigation and sensor blueprints.""" name: str - model_path: Path + urdf: UrdfLoader height_clearance: float width_clearance: float - internal_odom_offsets: dict[str, Any] = field(default_factory=dict) + # Lidar height above the ground when standing (used as the LIO init pose). + # This is a stance value, not a kinematic mount, so it is NOT in the URDF. + sensor_height: float + + @property + def static_transforms(self) -> dict[str, Transform]: + return self.urdf.static_transforms G1 = G1Config( name="unitree_g1", - model_path=Path(__file__).parent / "g1.urdf", + urdf=UrdfLoader(name="unitree_g1", model_path=Path(__file__).parent / "g1.urdf"), height_clearance=1.2, width_clearance=0.6, - internal_odom_offsets={ - # Mid-360 lidar: 1.2 m above ground. - "mid360_link": Pose(0.0, 0.0, 1.2, *Quaternion.from_euler(Vector3(0, 0, 0))), - }, + sensor_height=1.2, ) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py index be041b4788..9517992504 100644 --- a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py @@ -20,11 +20,10 @@ published continuously onto tf so they're captured in the recording. Raw Livox capture is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP stream. -The lidar IPs come from each module's own config (``DIMOS_MID360_LIDAR_IP`` for the -Mid-360 / pcap capture, ``DIMOS_POINTLIO_LIDAR_IP`` for Point-LIO). Run it for a -timestamped ``recordings/`` folder:: +The lidar IP comes from ``DIMOS_MID360_LIDAR_IP`` (shared by the Mid-360 / pcap +capture and Point-LIO). Run it for a timestamped ``recordings/`` folder:: - export DIMOS_MID360_LIDAR_IP=192.168.1.171 DIMOS_POINTLIO_LIDAR_IP=192.168.1.171 + export DIMOS_MID360_LIDAR_IP=192.168.1.171 uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record.py """ @@ -38,6 +37,7 @@ from dimos.hardware.sensors.lidar.livox.module import Mid360 from dimos.hardware.sensors.lidar.pointlio.module import PointLio from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder +from dimos.navigation.cmu_nav.frames import FRAME_ODOM from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.robot.unitree.go2.connection import GO2Connection from dimos.robot.unitree.go2.go2_mid360_recorder import Go2Mid360Recorder @@ -77,7 +77,7 @@ def _default_recording_dir() -> Path: (Mid360, "imu", "livox_imu"), ] ), - PointLio.blueprint(frame_id="world").remappings( + PointLio.blueprint(frame_mapping={FRAME_ODOM: "world"}).remappings( [ (PointLio, "lidar", "pointlio_lidar"), (PointLio, "odometry", "pointlio_odometry"), diff --git a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py index 1fae6b4ad3..120a6ae218 100644 --- a/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py +++ b/dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_nav_3d.py @@ -17,18 +17,34 @@ from typing import Any -from dimos.core.coordination.blueprints import autoconnect +from dimos.core.coordination.blueprints import Blueprint, autoconnect from dimos.core.global_config import global_config from dimos.hardware.sensors.lidar.pointlio.module import PointLio +from dimos.hardware.sensors.lidar.pointlio.mount_correction import mount_correction_matrix +from dimos.hardware.sensors.lidar.pointlio.recorder import PointlioRecorder +from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.navigation.basic_path_follower.module import BasicPathFollower from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.goal_relay import GoalRelay from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config +from dimos.robot.unitree.go2.config import mid360_rotated_urdf_path, mid360_urdf_path from dimos.robot.unitree.go2.connection import GO2Connection +from dimos.robot.urdf_loader import UrdfLoader from dimos.visualization.vis_module import vis_module +# Correction that un-rotates a physically-rotated Mid-360's Point-LIO cloud + +# odometry back into the normal mount frame. Used by the `_rotated` blueprint; the +# plain blueprint passes None (identity — sensor mounted normally). +_rotated_mount_correction = mount_correction_matrix( + original_static_tf=mid360_rotated_urdf_path, new_static_tf=mid360_urdf_path +) + +# GO2Connection is a TfModule; feeding it the normal mount publishes those frames +# on its static interval, matching the corrected cloud both blueprints emit. +go2_mid360_model = UrdfLoader(name="go2_mid360", model_path=mid360_urdf_path) + voxel_size = 0.08 # Height of the head-mounted lidar above the ground while standing. go2_lidar_height = 0.5 @@ -46,14 +62,34 @@ def _render_path(msg: Any) -> Any: return msg +def _render_surface_map(msg: Any) -> Any: + # Walkable surface the planner solved on. Cyan voxel boxes so it reads as a + # distinct carpet over the raw global_map. + return msg.to_rerun(voxel_size=voxel_size, colors=[80, 200, 255], mode="boxes") + + +def _render_nodes(msg: Any) -> Any: + # Planning-graph nodes as fat magenta spheres so they pop above the surface. + return msg.to_rerun(voxel_size=0.25, colors=[255, 0, 200], mode="spheres") + + +def _render_node_edges(msg: Any) -> Any: + # LineSegments3D already colors edges by traversability (green/yellow/red). + return msg.to_rerun() + + def _static_robot_body(rr: Any) -> list[Any]: - """Go2-shaped box on pointlio's body frame, counter-rotated for the lidar pitch.""" + """Go2-shaped box + forward arrow on pointlio's body frame. The hack fakes a + level, forward-facing mount, so the box needs no counter-rotation.""" return [ rr.Boxes3D(half_sizes=[0.35, 0.155, 0.2], colors=[(0, 255, 127)]), - rr.Transform3D( - parent_frame="tf#/body", - rotation=rr.RotationAxisAngle(axis=(0, 1, 0), degrees=-45.0), + # Red arrow out the nose marks the robot's forward (+x) direction. + rr.Arrows3D( + origins=[(0.35, 0.0, 0.0)], + vectors=[(0.3, 0.0, 0.0)], + colors=[(255, 40, 40)], ), + rr.Transform3D(parent_frame="tf#/body"), ] @@ -67,8 +103,10 @@ def _static_robot_body(rr: Any) -> list[Any]: "memory_limit": "256MB", # base_link tf comes from the go2 internal odometry, which is not the map # frame. Anchor the robot box to pointlio's body frame instead and hide the - # camera frustum that rides base_link. - "static": {"world/tf/body": _static_robot_body}, + # camera frustum that rides base_link. Use a dedicated entity path (not + # world/tf/body) so the box's Transform3D doesn't collide with the live + # odom->body TF that PointLio logs onto world/tf/body. + "static": {"world/robot_body": _static_robot_body}, "visual_override": { **rerun_config["visual_override"], "world/global_map": _render_global_map, @@ -76,43 +114,87 @@ def _static_robot_body(rr: Any) -> list[Any]: "world/camera_info": None, "world/color_image": None, "world/lidar": None, - "world/surface_map": None, - "world/nodes": None, - "world/node_edges": None, + "world/surface_map": _render_surface_map, + "world/nodes": _render_nodes, + "world/node_edges": _render_node_edges, }, } -unitree_go2_nav_3d = autoconnect( - vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), - # "mcf" for stair traversal - GO2Connection.blueprint(lidar=False, camera=False, motion_mode="mcf").remappings( - [ - (GO2Connection, "lidar", "lidar_l1"), - (GO2Connection, "odom", "odom_go2"), - ] - ), - PointLio.blueprint(body_frame_id="body"), - RayTracingVoxelMap.blueprint( - voxel_size=voxel_size, - emit_every=1, - global_emit_every=50, - max_health=10, - graze_cos=0.85, - ), - # global_map is remapped off so the planner runs purely on the - # incremental local_map + region_bounds pair. - MLSPlannerNative.blueprint( - world_frame="odom", - voxel_size=voxel_size, - robot_height=go2_lidar_height, - wall_clearance_m=0.2, - wall_buffer_m=0.75, - wall_buffer_weight=100.0, - step_threshold_m=0.16, - step_penalty_weight=1.0, - viz_publish_hz=0.0, - ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), - GoalRelay.blueprint(), - BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6), - MovementManager.blueprint(), -).global_config(n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False) + +def _nav_3d(mount_correction: list[float] | None) -> Blueprint: + """Go2 3D nav + recording stack. ``mount_correction`` un-rotates a physically + rotated Mid-360 back into the normal mount frame; None = normal mount (no + correction).""" + return autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_nav_rerun_config), + # "mcf" for stair traversal + GO2Connection.blueprint( + static_transforms=dict(go2_mid360_model.static_transforms), + lidar=False, + camera=False, + motion_mode="mcf", + ).remappings( + [ + (GO2Connection, "lidar", "lidar_l1"), + (GO2Connection, "odom", "odom_go2"), + ] + ), + # gravity_align is off so pointlio runs in the raw mount frame. For the + # rotated blueprint, transform rewrites its cloud + odometry (and odom->body + # TF) into the normal mount; otherwise transform is None (identity). + # auto_build recompiles the native binary for the transform support. + PointLio.blueprint( + gravity_align=False, + space_down_sample=False, + auto_build=True, + transform=mount_correction, + ), + # Record the corrected (normal-mount) cloud + odometry, not PointLio's raw + # tilted output, so a replay reproduces what the nav stack actually consumed. + # The remap only renames the wire topics to PointLio's lidar/odometry; the + # recorder's ports stay pointlio_lidar/pointlio_odometry, so that's what the + # db streams are named. + PointlioRecorder.blueprint().remappings( + [ + (PointlioRecorder, "pointlio_lidar", "lidar"), + (PointlioRecorder, "pointlio_odometry", "odometry"), + ] + ), + # Raw Livox UDP capture (tcpdump). Pulls the lidar IP + iface from env + # (DIMOS_MID360_LIDAR_IP / DIMOS_MID360_PCAP_IFACE); pcap lands in recordings/. + Mid360PcapRecorder.blueprint(), + RayTracingVoxelMap.blueprint( + voxel_size=voxel_size, + emit_every=1, + global_emit_every=50, + max_health=10, + graze_cos=0.85, + ), + # global_map is remapped off so the planner runs purely on the + # incremental local_map + region_bounds pair. + MLSPlannerNative.blueprint( + world_frame="odom", + voxel_size=voxel_size, + robot_height=go2_lidar_height, + wall_clearance_m=0.2, + wall_buffer_m=0.75, + wall_buffer_weight=100.0, + step_threshold_m=0.16, + step_penalty_weight=1.0, + viz_publish_hz=2.0, + ).remappings([(MLSPlannerNative, "global_map", "global_map_unused")]), + GoalRelay.blueprint(), + BasicPathFollower.blueprint(speed=0.5, heading_gain=0.4, max_angular=0.6), + MovementManager.blueprint(), + ) + + +# Standard mount (level, forward-facing): no correction. +unitree_go2_nav_3d = _nav_3d(None).global_config( + n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False +) + +# Rotated mount: un-rotate the cloud + odometry back into the normal mount frame. +unitree_go2_nav_3d_rotated = _nav_3d(_rotated_mount_correction).global_config( + n_workers=10, robot_model="unitree_go2", obstacle_avoidance=False +) diff --git a/dimos/robot/unitree/go2/config.py b/dimos/robot/unitree/go2/config.py new file mode 100644 index 0000000000..b1426e4604 --- /dev/null +++ b/dimos/robot/unitree/go2/config.py @@ -0,0 +1,71 @@ +# 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 importlib import resources +from pathlib import Path + +from dimos.constants import ( + DEFAULT_ROBOT_FRAME, + DEFAULT_WORLD_FRAME, +) +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.robot.urdf_loader import UrdfLoader + +_FRONT_CAMERA_720_YAML = resources.files("dimos.robot.unitree.go2").joinpath( + "front_camera_720.yaml" +) + + +def camera_info_static() -> CameraInfo: + with resources.as_file(_FRONT_CAMERA_720_YAML) as yaml_path: + return CameraInfo.from_yaml(str(yaml_path)) + + +go2_dir = Path(__file__).parent +mid360_urdf_path = go2_dir / "go2_mid360_normal.urdf" +mid360_rotated_urdf_path = go2_dir / "go2_mid360_rotated.urdf" + + +Go2Config = UrdfLoader( + name="unitree_go2", + model_path=go2_dir / "go2.urdf", +) + + +def odom_to_tf(odom: Odometry) -> list[Transform]: + """[world→base_link, base_link→camera_link, camera_link→camera_optical] for tests.""" + base_link = Transform( + translation=odom.position, + rotation=odom.orientation, + frame_id=odom.frame_id or DEFAULT_WORLD_FRAME, + child_frame_id=DEFAULT_ROBOT_FRAME, + ts=odom.ts, + ) + statics = [ + Transform( + translation=t.translation, + rotation=t.rotation, + frame_id=t.frame_id, + child_frame_id=t.child_frame_id, + ts=odom.ts, + ) + for t in ( + Go2Config.static_transforms["camera_link"], + Go2Config.static_transforms["camera_optical"], + Go2Config.static_transforms["lidar_link"], + ) + ] + return [base_link, *statics] diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 82826f417e..20a25187a2 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -13,9 +13,6 @@ # limitations under the License. from enum import Enum -from importlib import resources -import sys -from threading import Thread import time from typing import TYPE_CHECKING, Any, Protocol @@ -26,13 +23,16 @@ import rerun.blueprint as rrb from dimos.agents.annotation import skill -from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.constants import ( + DEFAULT_CAPACITY_COLOR_IMAGE, + DEFAULT_WORLD_FRAME, +) from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.core.core import rpc from dimos.core.global_config import GlobalConfig -from dimos.core.module import Module, ModuleConfig from dimos.core.resource import CompositeResource from dimos.core.stream import In, Out +from dimos.core.tf_module import TfModule, TfModuleConfig from dimos.core.transport import LCMTransport, pSHMTransport from dimos.spec.perception import Camera, Pointcloud from dimos.utils.logging_config import setup_logger @@ -42,22 +42,16 @@ from dimos.memory2.replay import Replay, resolve_db_path from dimos.memory2.store.sqlite import SqliteStore from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.robot.unitree.connection import UnitreeWebRTCConnection +from dimos.robot.unitree.go2.config import Go2Config, camera_info_static from dimos.robot.unitree.type.lowstate import LowStateMsg from dimos.utils.decorators.decorators import cached_property, simple_mcache -if sys.version_info < (3, 13): - from typing_extensions import TypeVar -else: - from typing import TypeVar - logger = setup_logger() @@ -66,7 +60,7 @@ class Go2Mode(str, Enum): RAGE = "rage" -class ConnectionConfig(ModuleConfig): +class ConnectionConfig(TfModuleConfig): ip: str = Field(default_factory=lambda m: m["g"].robot_ip) mode: Go2Mode = Go2Mode.DEFAULT lidar: bool = True @@ -75,6 +69,17 @@ class ConnectionConfig(ModuleConfig): motion_mode: str | None = None # Per-device AES-128 key (Go2 fw >=1.1.15); defaults from GlobalConfig. aes_128_key: str | None = Field(default_factory=lambda m: m["g"].unitree_aes_128_key) + frame_mapping: dict[str, str] = Field( + default_factory=lambda: dict( + body=Go2Config.body_frame, + parent=DEFAULT_WORLD_FRAME, + camera_link="camera_link", + camera_optical="camera_optical", + ) + ) + static_transforms: dict[str, Transform] = Field( + default_factory=lambda: dict(Go2Config.static_transforms) + ) class Go2ConnectionProtocol(Protocol): @@ -95,31 +100,6 @@ def set_rage_mode(self, enable: bool) -> bool: ... def publish_request(self, topic: str, data: dict) -> dict: ... # type: ignore[type-arg] -_FRONT_CAMERA_720_YAML = resources.files("dimos.robot.unitree.go2").joinpath( - "front_camera_720.yaml" -) - - -def _camera_info_static() -> CameraInfo: - with resources.as_file(_FRONT_CAMERA_720_YAML) as yaml_path: - return CameraInfo.from_yaml(str(yaml_path)) - - -# Static camera mount chain: base_link -> camera_link -> camera_optical. -# TODO we need a standardized way to specify this for all cameras in dimos -BASE_TO_OPTICAL: Transform = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", -) + Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", -) - - def make_connection( ip: str | None, cfg: GlobalConfig, @@ -215,10 +195,7 @@ def publish_request(self, topic: str, data: dict): # type: ignore[no-untyped-de return {"status": "ok", "message": "Fake publish"} -_Config = TypeVar("_Config", bound=ConnectionConfig, default=ConnectionConfig) - - -class GO2Connection(Module, Camera, Pointcloud): +class GO2Connection(TfModule, Camera, Pointcloud): dedicated_worker = True config: ConnectionConfig @@ -230,8 +207,7 @@ class GO2Connection(Module, Camera, Pointcloud): camera_info: Out[CameraInfo] connection: Go2ConnectionProtocol - camera_info_static: CameraInfo = _camera_info_static() - _camera_info_thread: Thread | None = None + camera_info_static: CameraInfo = camera_info_static() _latest_video_frame: Image | None = None _latest_lowstate: LowStateMsg | None = None @@ -250,9 +226,10 @@ def __init__(self, **kwargs: Any) -> None: self.connection = make_connection( self.config.ip, self.config.g, aes_128_key=self.config.aes_128_key ) + self._camera_info_static = camera_info_static() if hasattr(self.connection, "camera_info_static"): - self.camera_info_static = self.connection.camera_info_static + self._camera_info_static = self.connection.camera_info_static @rpc def start(self) -> None: @@ -261,7 +238,8 @@ def start(self) -> None: return self.connection.start() - def onimage(image: Image) -> None: + def on_image(image: Image) -> None: + image.frame_id = self.frame_mapping["camera_optical"] self.color_image.publish(image) self._latest_video_frame = image @@ -272,12 +250,7 @@ def onimage(image: Image) -> None: self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) if self.config.camera: - self.register_disposable(self.connection.video_stream().subscribe(onimage)) - self._camera_info_thread = Thread( - target=self.publish_camera_info, - daemon=True, - ) - self._camera_info_thread.start() + self.register_disposable(self.connection.video_stream().subscribe(on_image)) if self.config.motion_mode and isinstance(self.connection, UnitreeWebRTCConnection): self.connection.set_motion_mode(self.config.motion_mode) @@ -298,46 +271,26 @@ def stop(self) -> None: if self.connection: self.connection.stop() - if self._camera_info_thread and self._camera_info_thread.is_alive(): - self._camera_info_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - super().stop() - @classmethod - def _odom_to_tf(cls, odom: PoseStamped) -> list[Transform]: - camera_link = Transform( - translation=Vector3(0.3, 0.0, 0.0), - rotation=Quaternion(0.0, 0.0, 0.0, 1.0), - frame_id="base_link", - child_frame_id="camera_link", - ts=odom.ts, - ) - - camera_optical = Transform( - translation=Vector3(0.0, 0.0, 0.0), - rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), - frame_id="camera_link", - child_frame_id="camera_optical", - ts=odom.ts, - ) - - return [ - Transform.from_pose("base_link", odom), - camera_link, - camera_optical, - ] + def _on_static_publish(self) -> None: + self._camera_info_static.frame_id = self.frame_mapping["camera_optical"] + self.camera_info.publish(self._camera_info_static) def _publish_tf(self, msg: PoseStamped) -> None: - transforms = self._odom_to_tf(msg) - self.tf.publish(*transforms) + self.tf.publish( + Transform( + translation=msg.position, + rotation=msg.orientation, + frame_id=self.frame_mapping["parent"], + child_frame_id=self.frame_mapping["body"], + ts=msg.ts, + ) + ) if self.odom.transport: + msg.frame_id = self.frame_mapping["parent"] self.odom.publish(msg) - def publish_camera_info(self) -> None: - while True: - self.camera_info.publish(self.camera_info_static) - time.sleep(1.0) - @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: """Send movement command to robot.""" @@ -406,8 +359,6 @@ def observe(self) -> Image | None: def deploy(dimos: ModuleCoordinator, ip: str, prefix: str = "") -> "ModuleProxy": - from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE - connection = dimos.deploy(GO2Connection, ip=ip) connection.pointcloud.transport = pSHMTransport( diff --git a/dimos/robot/unitree/go2/go2.urdf b/dimos/robot/unitree/go2/go2.urdf index 4e67e9ca8e..91c914389b 100644 --- a/dimos/robot/unitree/go2/go2.urdf +++ b/dimos/robot/unitree/go2/go2.urdf @@ -19,4 +19,26 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/unitree/go2/go2_mid360_normal.urdf b/dimos/robot/unitree/go2/go2_mid360_normal.urdf new file mode 100644 index 0000000000..3a4ea01189 --- /dev/null +++ b/dimos/robot/unitree/go2/go2_mid360_normal.urdf @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/unitree/go2/go2_mid360_rotated.urdf b/dimos/robot/unitree/go2/go2_mid360_rotated.urdf new file mode 100644 index 0000000000..c17d084ee1 --- /dev/null +++ b/dimos/robot/unitree/go2/go2_mid360_rotated.urdf @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/dimos/robot/urdf_loader.py b/dimos/robot/urdf_loader.py new file mode 100644 index 0000000000..6d9e609aee --- /dev/null +++ b/dimos/robot/urdf_loader.py @@ -0,0 +1,115 @@ +# 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. + +"""URDF/MJCF frame loading. + +The model file is the ground truth for a robot's frames. `UrdfLoader` parses it +lazily and derives the frame information dimos needs (the structural body frame +and the fixed-joint static transforms) without pulling in any control, +manipulation, or hardware machinery. +""" + +from __future__ import annotations + +from functools import cached_property +from pathlib import Path + +from pydantic import BaseModel, Field, PrivateAttr + +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.model_parser import JointDescription, ModelDescription, parse_model + + +class UrdfLoader(BaseModel): + """Lazily parses a URDF/MJCF model and exposes its derived frame info. + + Parsing is deferred until first access so importing a config that references + a model never triggers an LFS download or disk read. + """ + + name: str + model_path: Path | None = None + package_paths: dict[str, Path] = Field(default_factory=dict) + xacro_args: dict[str, str] = Field(default_factory=dict) + + _parsed: ModelDescription | None = PrivateAttr(default=None) + + def _ensure_parsed(self) -> ModelDescription: + if self._parsed is None: + if self.model_path is None: + raise ValueError( + f"UrdfLoader {self.name!r} has no model_path — frame info is unavailable. " + "Set model_path to a URDF/MJCF." + ) + self._parsed = parse_model(self.model_path, self.package_paths, self.xacro_args) + return self._parsed + + @property + def model_description(self) -> ModelDescription: + return self._ensure_parsed() + + @cached_property + def all_frame_ids(self) -> list[str]: + return list(self._ensure_parsed().links) + + @cached_property + def body_frame(self) -> str: + """The robot's structural root link (usually ``base_link``). + + Skips past ``type="floating"`` joints and returns the first structural + link. (``world`` can be the true root frame, but it is detached from the + robot, so it is not the body frame.) + """ + model = self._ensure_parsed() + outgoing: dict[str, list[JointDescription]] = {} + for joint in model.joints: + outgoing.setdefault(joint.parent_link, []).append(joint) + current = model.root_link + while True: + floating_joint = next( + (joint for joint in outgoing.get(current, []) if joint.type == "floating"), + None, + ) + if floating_joint is None: + return current + current = floating_joint.child_link + + @cached_property + def static_transforms(self) -> dict[str, Transform]: + """Fixed-joint transforms keyed by child frame (parent → child). + + Example:: + print(UrdfLoader(name="go2", model_path="go2.urdf").static_transforms) + # { + # "camera_link": Transform(translation=(0.3, 0, 0), rotation=identity, + # frame_id="base_link", child_frame_id="camera_link"), + # "camera_optical": Transform(translation=(0, 0, 0), rotation=(-0.5, 0.5, -0.5, 0.5), + # frame_id="camera_link", child_frame_id="camera_optical"), + # } + """ + result: dict[str, Transform] = {} + for joint in self._ensure_parsed().joints: + if joint.type != "fixed" or not joint.child_link: + continue + roll, pitch, yaw = joint.origin_rpy + tx, ty, tz = joint.origin_xyz + result[joint.child_link] = Transform( + translation=Vector3(tx, ty, tz), + rotation=Quaternion.from_euler(Vector3(roll, pitch, yaw)), + frame_id=joint.parent_link, + child_frame_id=joint.child_link, + ) + return result diff --git a/dimos/utils/testing/test_moment.py b/dimos/utils/testing/test_moment.py index 7518a1af02..ac6b69ced8 100644 --- a/dimos/utils/testing/test_moment.py +++ b/dimos/utils/testing/test_moment.py @@ -22,7 +22,7 @@ from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.protocol.tf.tf import TF -from dimos.robot.unitree.go2 import connection +from dimos.robot.unitree.go2.config import camera_info_static, odom_to_tf from dimos.utils.data import get_data from dimos.utils.testing.moment import Moment, SensorMoment @@ -51,14 +51,14 @@ def transforms(self) -> list[Transform]: # back and forth through time and the viewer doesn't get confused odom = self.odom.value odom.ts = time.time() - return connection.GO2Connection._odom_to_tf(odom) + return odom_to_tf(odom) def publish(self) -> None: t = TF() t.publish(*self.transforms) t.stop() - camera_info = connection._camera_info_static() + camera_info = camera_info_static() camera_info.ts = time.time() camera_info_transport: LCMTransport[CameraInfo] = LCMTransport("/camera_info", CameraInfo) camera_info_transport.publish(camera_info) diff --git a/docs/capabilities/memory/plot.md b/docs/capabilities/memory/plot.md index 97d45b5423..6d491147aa 100644 --- a/docs/capabilities/memory/plot.md +++ b/docs/capabilities/memory/plot.md @@ -357,10 +357,7 @@ m.data.save("assets/plants_peak_detections.png") from dimos.perception.detection.type.detection3d.imageDetections3DPC import ( ImageDetections3DPC, ) -from dimos.robot.unitree.go2.connection import ( - _camera_info_static as go2_camerainfo, - BASE_TO_OPTICAL, -) +from dimos.robot.unitree.go2.config import Go2Config, camera_info_static from dimos.memory2.vis.space.elements import Box3D from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.Transform import Transform @@ -369,7 +366,7 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 # TODO We need a nicer way to get optical transform for image streams # depending on the source def world_to_optical(base_pose): - return -(Transform.from_pose("base_link", base_pose) + BASE_TO_OPTICAL) + return -(Transform.from_pose("base_link", base_pose) + Go2Config.static_transforms["camera_link"] + Go2Config.static_transforms["camera_optical"]) drawing = Space() @@ -377,7 +374,7 @@ drawing.add(global_map) drawing.add(detections) -camera_info = go2_camerainfo() +camera_info = camera_info_static() detections3d = (detections .map_data(lambda obs: ImageDetections3DPC.from_2d( diff --git a/docs/development/conventions.md b/docs/development/conventions.md index 53259c74da..bacdf6f84e 100644 --- a/docs/development/conventions.md +++ b/docs/development/conventions.md @@ -1,5 +1,6 @@ This mostly to track when conventions change (with regard to codebase updates) because this codebase is under heavy development. Note: this is a non-exhaustive list of conventions. +- We no longer use `__all__`, do not include it in any python file - Instead of using `RerunBridge` in blueprints we always use `vis_module` which allows the CLI to control if its rerun or no-vis at all - When global_config.py shouldn't accidentally/indirectly import heavy libraries like rerun. But sometimes global_config needs the type definition or default value from a module. Preferably we import from the module file directly, however when thats not possible, we create a config.py for just that module's config and import that into global_config.py. - When adding visualization tools to a blueprint/autoconnect, instead of using RerunBridge or WebsocketVisModule directly we should always use `vis_module`, which right now should look something like `vis_module(viewer_backend=global_config.viewer, rerun_config={}),` diff --git a/pyproject.toml b/pyproject.toml index 3fb41fd4f6..fb70bc0440 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -128,7 +128,7 @@ dependencies = [ "toolz>=1.1.0", "protobuf>=6.33.5,<7", "psutil>=7.0.0", - "sqlite-vec>=0.1.6", + "sqlite-vec>=0.1.7", "lz4>=4.4.5", "bleak>=3.0.2", "cryptography>=46.0.5", diff --git a/uv.lock b/uv.lock index 24c657c3b8..f1b579053b 100644 --- a/uv.lock +++ b/uv.lock @@ -2044,7 +2044,7 @@ requires-dist = [ { name = "sortedcontainers", specifier = "==2.4.0" }, { name = "sounddevice", marker = "extra == 'agents'" }, { name = "soundfile", marker = "extra == 'web'" }, - { name = "sqlite-vec", specifier = ">=0.1.6" }, + { name = "sqlite-vec", specifier = ">=0.1.7" }, { name = "sse-starlette", marker = "extra == 'web'", specifier = ">=2.2.1" }, { name = "structlog", specifier = ">=25.5.0,<26" }, { name = "tensorboard", marker = "extra == 'misc'", specifier = "==2.20.0" }, @@ -8146,14 +8146,14 @@ wheels = [ [[package]] name = "sqlite-vec" -version = "0.1.6" +version = "0.1.9" source = { registry = "https://pypi.org/simple" } wheels = [ - { url = "https://files.pythonhosted.org/packages/88/ed/aabc328f29ee6814033d008ec43e44f2c595447d9cccd5f2aabe60df2933/sqlite_vec-0.1.6-py3-none-macosx_10_6_x86_64.whl", hash = "sha256:77491bcaa6d496f2acb5cc0d0ff0b8964434f141523c121e313f9a7d8088dee3", size = 164075, upload-time = "2024-11-20T16:40:29.847Z" }, - { url = "https://files.pythonhosted.org/packages/a7/57/05604e509a129b22e303758bfa062c19afb020557d5e19b008c64016704e/sqlite_vec-0.1.6-py3-none-macosx_11_0_arm64.whl", hash = "sha256:fdca35f7ee3243668a055255d4dee4dea7eed5a06da8cad409f89facf4595361", size = 165242, upload-time = "2024-11-20T16:40:31.206Z" }, - { url = "https://files.pythonhosted.org/packages/f2/48/dbb2cc4e5bad88c89c7bb296e2d0a8df58aab9edc75853728c361eefc24f/sqlite_vec-0.1.6-py3-none-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:7b0519d9cd96164cd2e08e8eed225197f9cd2f0be82cb04567692a0a4be02da3", size = 103704, upload-time = "2024-11-20T16:40:33.729Z" }, - { url = "https://files.pythonhosted.org/packages/80/76/97f33b1a2446f6ae55e59b33869bed4eafaf59b7f4c662c8d9491b6a714a/sqlite_vec-0.1.6-py3-none-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux1_x86_64.whl", hash = "sha256:823b0493add80d7fe82ab0fe25df7c0703f4752941aee1c7b2b02cec9656cb24", size = 151556, upload-time = "2024-11-20T16:40:35.387Z" }, - { url = "https://files.pythonhosted.org/packages/6a/98/e8bc58b178266eae2fcf4c9c7a8303a8d41164d781b32d71097924a6bebe/sqlite_vec-0.1.6-py3-none-win_amd64.whl", hash = "sha256:c65bcfd90fa2f41f9000052bcb8bb75d38240b2dae49225389eca6c3136d3f0c", size = 281540, upload-time = "2024-11-20T16:40:37.296Z" }, + { url = "https://files.pythonhosted.org/packages/68/85/9fad0045d8e7c8df3e0fa5a56c630e8e15ad6e5ca2e6106fceb666aa6638/sqlite_vec-0.1.9-py3-none-macosx_10_6_x86_64.whl", hash = "sha256:1b62a7f0a060d9475575d4e599bbf94a13d85af896bc1ce86ee80d1b5b48e5fb", size = 131171, upload-time = "2026-03-31T08:02:31.717Z" }, + { url = "https://files.pythonhosted.org/packages/a4/3d/3677e0cd2f92e5ebc43cd29fbf565b75582bff1ccfa0b8327c7508e1084f/sqlite_vec-0.1.9-py3-none-macosx_11_0_arm64.whl", hash = "sha256:1d52e30513bae4cc9778ddbf6145610434081be4c3afe57cd877893bad9f6b6c", size = 165434, upload-time = "2026-03-31T08:02:32.712Z" }, + { url = "https://files.pythonhosted.org/packages/00/d4/f2b936d3bdc38eadcbd2a87875815db36430fab0363182ba5d12cd8e0b51/sqlite_vec-0.1.9-py3-none-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:4e921e592f24a5f9a18f590b6ddd530eb637e2d474e3b1972f9bbeb773aa3cb9", size = 160076, upload-time = "2026-03-31T08:02:33.796Z" }, + { url = "https://files.pythonhosted.org/packages/6f/ad/6afd073b0f817b3e03f9e37ad626ae341805891f23c74b5292818f49ac63/sqlite_vec-0.1.9-py3-none-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux1_x86_64.whl", hash = "sha256:1515727990b49e79bcaf75fdee2ffc7d461f8b66905013231251f1c8938e7786", size = 163388, upload-time = "2026-03-31T08:02:34.888Z" }, + { url = "https://files.pythonhosted.org/packages/42/89/81b2907cda14e566b9bf215e2ad82fc9b349edf07d2010756ffdb902f328/sqlite_vec-0.1.9-py3-none-win_amd64.whl", hash = "sha256:4a28dc12fa4b53d7b1dced22da2488fade444e96b5d16fd2d698cd670675cf32", size = 292804, upload-time = "2026-03-31T08:02:36.035Z" }, ] [[package]]