From a74886f0381b93413b24f79fb716b938a54c0093 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 28 Aug 2026 17:24:04 +0800 Subject: [PATCH 01/15] feat(robot): add native M20 KronkNav integration --- dimos/robot/all_blueprints.py | 4 + dimos/robot/deeprobotics/__init__.py | 15 + dimos/robot/deeprobotics/m20/__init__.py | 15 + .../deeprobotics/m20/blueprints/__init__.py | 15 + .../m20/blueprints/m20_kronknav.py | 146 ++++++ .../m20/blueprints/test_m20_kronknav.py | 101 +++++ dimos/robot/deeprobotics/m20/bridge/README.md | 41 ++ .../robot/deeprobotics/m20/bridge/__init__.py | 19 + .../m20/bridge/cpp/CMakeLists.txt | 56 +++ .../deeprobotics/m20/bridge/cpp/build.sh | 35 ++ .../deeprobotics/m20/bridge/cpp/main.cpp | 424 ++++++++++++++++++ dimos/robot/deeprobotics/m20/bridge/module.py | 85 ++++ dimos/robot/deeprobotics/m20/connection.py | 171 +++++++ dimos/robot/deeprobotics/m20/constants.py | 37 ++ .../robot/deeprobotics/m20/test_connection.py | 173 +++++++ 15 files changed, 1337 insertions(+) create mode 100644 dimos/robot/deeprobotics/__init__.py create mode 100644 dimos/robot/deeprobotics/m20/__init__.py create mode 100644 dimos/robot/deeprobotics/m20/blueprints/__init__.py create mode 100644 dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py create mode 100644 dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py create mode 100644 dimos/robot/deeprobotics/m20/bridge/README.md create mode 100644 dimos/robot/deeprobotics/m20/bridge/__init__.py create mode 100644 dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt create mode 100755 dimos/robot/deeprobotics/m20/bridge/cpp/build.sh create mode 100644 dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp create mode 100644 dimos/robot/deeprobotics/m20/bridge/module.py create mode 100644 dimos/robot/deeprobotics/m20/connection.py create mode 100644 dimos/robot/deeprobotics/m20/constants.py create mode 100644 dimos/robot/deeprobotics/m20/test_connection.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index e39b3213df..eac1ce3173 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -42,6 +42,8 @@ "coordinator-velocity-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_velocity_xarm6", "coordinator-xarm6": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm6", "coordinator-xarm7": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm7", + "deeprobotics-m20-kronknav": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav", + "deeprobotics-m20-kronknav-control": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav_control", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", @@ -226,6 +228,8 @@ "joystick-module": "dimos.robot.unitree.b1.joystick_module.JoystickModule", "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", + "m20-connection": "dimos.robot.deeprobotics.m20.connection.M20Connection", + "m20-ros-bridge": "dimos.robot.deeprobotics.m20.bridge.module.M20ROSBridge", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "manipulation-skills": "dimos.manipulation.manipulation_skills.ManipulationSkills", "map": "dimos.robot.unitree.type.map.Map", diff --git a/dimos/robot/deeprobotics/__init__.py b/dimos/robot/deeprobotics/__init__.py new file mode 100644 index 0000000000..6cf15a5bc4 --- /dev/null +++ b/dimos/robot/deeprobotics/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""Deep Robotics integrations.""" diff --git a/dimos/robot/deeprobotics/m20/__init__.py b/dimos/robot/deeprobotics/m20/__init__.py new file mode 100644 index 0000000000..624498841a --- /dev/null +++ b/dimos/robot/deeprobotics/m20/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""Deep Robotics Lynx M20 integration.""" diff --git a/dimos/robot/deeprobotics/m20/blueprints/__init__.py b/dimos/robot/deeprobotics/m20/blueprints/__init__.py new file mode 100644 index 0000000000..4e1ae872dc --- /dev/null +++ b/dimos/robot/deeprobotics/m20/blueprints/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""Runnable M20 blueprints.""" diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py new file mode 100644 index 0000000000..6c3dd104a3 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -0,0 +1,146 @@ +# 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. + +"""Robot-local M20 integration for the current DimOS 3D navigation stack.""" + +from typing import Any + +from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.global_config import global_config +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC +from dimos.navigation.dannav.local_planner.module import DanLocalPlanner +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.navigation.nav_3d.mls_planner.viz import planner_visual_override +from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge +from dimos.robot.deeprobotics.m20.connection import M20Connection +from dimos.robot.deeprobotics.m20.constants import ( + BASE_LINK_HEIGHT_M, + BODY_WIDTH_M, + MAX_ANGULAR_Z_RAD_S, + MAX_LINEAR_X_M_S, + MAX_LINEAR_Y_M_S, + PLANNING_HEIGHT_M, + ROTATION_DIAMETER_M, +) +from dimos.visualization.vis_module import vis_module + +VOXEL_SIZE_M = 0.1 +PLANNER_VIZ_HZ = 0.0 + +CRUISE_SPEED_M_S = 0.25 + + +def _render_global_map(msg: Any) -> Any: + return msg.to_rerun() + + +def _render_path(msg: Any) -> Any: + if len(msg.poses) == 0: + return None + return msg + + +_rerun_config = { + "memory_limit": "256MB", + "tf_axes": 0.35, + "max_hz": { + "world/lidar": 2.0, + "world/local_map": 2.0, + "world/global_map": 0.2, + }, + "visual_override": { + "world/global_map": _render_global_map, + "world/planner_path": None, + "world/path": _render_path, + **planner_visual_override(PLANNER_VIZ_HZ), + }, +} + + +def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: + """Compose one complete M20 graph on GOS. + + The boolean is intentionally fixed by the two exported blueprints below; + selecting the control blueprint is the deployment-time ownership decision. + Both still start with the Python command gate disarmed. + """ + return autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + M20ROSBridge.blueprint( + enable_command_output=enable_command_output, + max_linear_x=MAX_LINEAR_X_M_S, + max_linear_y=MAX_LINEAR_Y_M_S, + max_angular_z=MAX_ANGULAR_Z_RAD_S, + ), + M20Connection.blueprint( + max_linear_x=MAX_LINEAR_X_M_S, + max_linear_y=MAX_LINEAR_Y_M_S, + max_angular_z=MAX_ANGULAR_Z_RAD_S, + ), + RayTracingVoxelMap.blueprint( + voxel_size=VOXEL_SIZE_M, + max_range=25.0, + emit_every=1, + global_emit_every=20, + support_min=4, + world_frame="odom", + worker_threads=3, + ), + MLSPlannerNative.blueprint( + world_frame="odom", + base_frame="base_link", + voxel_size=VOXEL_SIZE_M, + robot_height=PLANNING_HEIGHT_M, + start_z_offset_m=BASE_LINK_HEIGHT_M, + wall_clearance_m=0.3, + wall_buffer_m=0.85, + wall_buffer_weight=100.0, + step_threshold_m=0.12, + step_penalty_weight=4.0, + viz_publish_hz=PLANNER_VIZ_HZ, + worker_threads=2, + ).remappings( + [ + (MLSPlannerNative, "global_map", "global_map_unused"), + (MLSPlannerNative, "path", "planner_path"), + ] + ), + DanLocalPlanner.blueprint( + lock_replan=0.4, + resample_spacing_m=0.1, + ), + DanHolonomicTC.blueprint( + run_profile="walk", + speed_m_s=CRUISE_SPEED_M_S, + control_frequency=10.0, + ), + MovementManager.blueprint(), + ).global_config( + n_workers=4, + obstacle_avoidance=False, + robot_width=BODY_WIDTH_M, + robot_rotation_diameter=ROTATION_DIAMETER_M, + transport="lcm", + ) + + +# Safe default: mapping, planning, and Rerun are live, but the native process +# does not create a /NAV_CMD publisher and M20Connection can never become ready. +deeprobotics_m20_kronknav = autoconnect(_m20_kronknav(enable_command_output=False)) + +# Explicit control ownership: creates /NAV_CMD, while still requiring fresh +# estop/localization status and a deliberate M20Connection.arm() RPC. +deeprobotics_m20_kronknav_control = autoconnect(_m20_kronknav(enable_command_output=True)) diff --git a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py new file mode 100644 index 0000000000..10ef55b732 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py @@ -0,0 +1,101 @@ +# 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. + +"""Safety contract tests for the exported M20 blueprints.""" + +from dimos.core.coordination.blueprints import Blueprint +from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC +from dimos.navigation.dannav.local_planner.module import DanLocalPlanner +from dimos.navigation.movement_manager.movement_manager import MovementManager +from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative +from dimos.robot.deeprobotics.m20.blueprints.m20_kronknav import ( + deeprobotics_m20_kronknav, + deeprobotics_m20_kronknav_control, +) +from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge +from dimos.robot.deeprobotics.m20.connection import M20Connection + + +def _bridge_kwargs(blueprint: Blueprint) -> dict[str, object]: + atoms = [atom for atom in blueprint.blueprints if atom.module is M20ROSBridge] + assert len(atoms) == 1 + return atoms[0].kwargs + + +def _endpoint_modules( + blueprint: Blueprint, + name: str, + stream_type: type, + direction: str, +) -> set[type]: + result: set[type] = set() + for atom in blueprint.active_blueprints: + for stream in atom.streams: + effective_name = blueprint.remapping_map.get((atom.name, stream.name), stream.name) + if ( + effective_name == name + and stream.type is stream_type + and stream.direction == direction + ): + result.add(atom.module) + return result + + +def test_default_kronknav_blueprint_cannot_publish_robot_commands() -> None: + assert _bridge_kwargs(deeprobotics_m20_kronknav)["enable_command_output"] is False + + +def test_control_blueprint_explicitly_enables_robot_command_publisher() -> None: + assert _bridge_kwargs(deeprobotics_m20_kronknav_control)["enable_command_output"] is True + + +def test_m20_blueprints_pin_native_sdk_supported_local_transport() -> None: + assert deeprobotics_m20_kronknav.global_config_overrides["transport"] == "lcm" + assert deeprobotics_m20_kronknav_control.global_config_overrides["transport"] == "lcm" + + +def test_sensor_and_pose_streams_reach_mapping_and_navigation() -> None: + blueprint = deeprobotics_m20_kronknav + + assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20ROSBridge} + assert RayTracingVoxelMap in _endpoint_modules(blueprint, "lidar", PointCloud2, "in") + assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "tf", TFMessage, "in") == { + RayTracingVoxelMap, + MLSPlannerNative, + } + assert _endpoint_modules(blueprint, "odom", PoseStamped, "out") == {M20ROSBridge} + assert { + DanLocalPlanner, + DanHolonomicTC, + } <= _endpoint_modules(blueprint, "odom", PoseStamped, "in") + + +def test_kronknav_path_and_guarded_command_chain_is_complete() -> None: + blueprint = deeprobotics_m20_kronknav_control + + assert _endpoint_modules(blueprint, "planner_path", Path, "out") == {MLSPlannerNative} + assert _endpoint_modules(blueprint, "planner_path", Path, "in") == {DanLocalPlanner} + assert _endpoint_modules(blueprint, "path", Path, "out") == {DanLocalPlanner} + assert DanHolonomicTC in _endpoint_modules(blueprint, "path", Path, "in") + assert _endpoint_modules(blueprint, "cmd_vel", Twist, "out") == {MovementManager} + assert _endpoint_modules(blueprint, "cmd_vel", Twist, "in") == {M20Connection} + assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "out") == {M20Connection} + assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "in") == {M20ROSBridge} diff --git a/dimos/robot/deeprobotics/m20/bridge/README.md b/dimos/robot/deeprobotics/m20/bridge/README.md new file mode 100644 index 0000000000..2b12ab3386 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/README.md @@ -0,0 +1,41 @@ +# M20 robot-local ROS bridge + +This native module is built and run on the M20 Pro GOS computer. It converts the +vendor's local ROS 2/DrDDS topics into typed DimOS streams without a Python +`rclpy` dependency or another application host. + +The current C++ NativeModule SDK carries those streams over local LCM. The M20 +blueprints therefore pin the complete onboard graph to LCM; this bridge does not +contain a private Zenoh implementation. + +The bridge always subscribes to `/LIDAR/POINTS`, `/ODOM`, `/LOCATION_STATUS`, +and `/HES_STATUS`. It publishes lidar, pose, odometry, TF, and command-readiness +streams into the local DimOS graph. + +`enable_command_output` defaults to `false`. When explicitly enabled, the bridge +owns a `/NAV_CMD` publisher but emits nonzero velocity only while: + +- location status is fresh and exactly `1` (normal); +- hard-estop status is fresh and exactly `0` (not triggered); +- the `/NAV_CMD` publisher has a matched subscriber; +- the Python connection has explicitly armed and supplied a fresh bounded command. + +The native watchdog uses a steady clock and sends zero after command timeout, +on health loss, and during shutdown. Starting the bridge never changes robot +mode, gait, planner service, charging state, or standing state. + +Build on GOS after sourcing the vendor environment: + +```bash +./build.sh +``` + +Deploy from a DimOS source checkout. The Python wheel does not include this C++ +source tree or the in-repo native SDK, both of which the robot-local build uses. + +The default setup path is `/opt/robot/scripts/setup_ros2.sh`; override it with +`M20_ROS_SETUP` if the inspected robot differs. The build intentionally fails +off-robot when Foxy and the installed `drdds` message package are unavailable. +It also requires CMake, a C++20 compiler, pkg-config, and the LCM development +package. For an offline build, clone the pinned `dimos-lcm` revision separately +and set `DIMOS_LCM_DIR` to that checkout before running `build.sh`. diff --git a/dimos/robot/deeprobotics/m20/bridge/__init__.py b/dimos/robot/deeprobotics/m20/bridge/__init__.py new file mode 100644 index 0000000000..a357d1f6f8 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/__init__.py @@ -0,0 +1,19 @@ +# 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. + +"""Robot-local ROS 2/DrDDS bridge for the Lynx M20.""" + +from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge + +__all__ = ["M20ROSBridge"] diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt new file mode 100644 index 0000000000..1f3f4fc9fb --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt @@ -0,0 +1,56 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.14) +project(m20_ros_bridge CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(ament_cmake REQUIRED) +find_package(drdds REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) + +if(DEFINED DIMOS_LCM_DIR) + set(dimos_lcm_SOURCE_DIR ${DIMOS_LCM_DIR}) +else() + include(FetchContent) + FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG 0a1c24710ce2f7a569e1673617102cc5254a75e2 + ) + # dimos-lcm is a generated-header repository without a CMakeLists.txt, so it + # can be populated but not added as a CMake subproject. + FetchContent_GetProperties(dimos_lcm) + if(NOT dimos_lcm_POPULATED) + FetchContent_Populate(dimos_lcm) + endif() +endif() + +if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) + set(DIMOS_NATIVE_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../native/cpp) +endif() +add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) + +add_executable(m20_ros_bridge main.cpp) +target_include_directories(m20_ros_bridge PRIVATE + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs +) +ament_target_dependencies(m20_ros_bridge + drdds + nav_msgs + rclcpp + sensor_msgs +) +target_link_libraries(m20_ros_bridge PRIVATE + dimos_native + PkgConfig::LCM +) +target_compile_options(m20_ros_bridge PRIVATE -Wall -Wextra -Wpedantic) + +install(TARGETS m20_ros_bridge DESTINATION bin) diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh b/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh new file mode 100755 index 0000000000..e4a131f2d0 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh @@ -0,0 +1,35 @@ +#!/usr/bin/env bash +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +bridge_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ros_setup="${M20_ROS_SETUP:-/opt/robot/scripts/setup_ros2.sh}" + +if [[ -f "$ros_setup" ]]; then + # shellcheck disable=SC1090 + set +u + source "$ros_setup" + set -u +elif [[ -f /opt/ros/foxy/setup.bash ]]; then + # shellcheck disable=SC1091 + set +u + source /opt/ros/foxy/setup.bash + set -u +else + echo "M20 ROS setup not found: $ros_setup" >&2 + exit 1 +fi + +cmake_args=( + -S "$bridge_dir" + -B "$bridge_dir/build" + -DCMAKE_BUILD_TYPE=Release +) +if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then + cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") +fi + +cmake "${cmake_args[@]}" +cmake --build "$bridge_dir/build" --parallel "${M20_BUILD_JOBS:-4}" diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp new file mode 100644 index 0000000000..7bc44a63b4 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp @@ -0,0 +1,424 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// Robot-local ROS 2/DrDDS adapter for the Deep Robotics Lynx M20. +// DimOS and this process both run on GOS. ROS 2 is used only to reach the +// vendor topics; typed DimOS streams use the native SDK's local LCM transport. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "dimos/native.hpp" + +#include "geometry_msgs/PoseStamped.hpp" +#include "geometry_msgs/TransformStamped.hpp" +#include "geometry_msgs/Twist.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Bool.hpp" +#include "std_msgs/Header.hpp" +#include "tf2_msgs/TFMessage.hpp" + +using dimos::native::Builder; +using dimos::native::Config; +using dimos::native::Module; +using dimos::native::Output; +namespace logging = dimos::native::log; + +namespace { + +using Clock = std::chrono::steady_clock; +constexpr int64_t kNanosecondsPerSecond = 1'000'000'000LL; + +void require_nonempty(const std::string& value, const char* name) { + if (value.empty()) { + throw std::runtime_error(std::string(name) + " must not be empty"); + } +} + +int32_t checked_i32(std::size_t value, const char* name) { + if (value > static_cast(std::numeric_limits::max())) { + throw std::runtime_error(std::string(name) + " exceeds the DimOS message limit"); + } + return static_cast(value); +} + +double clamp(double value, double limit) { + return std::max(-limit, std::min(limit, value)); +} + +geometry_msgs::Twist zero_twist() { + geometry_msgs::Twist result; + result.linear.x = 0.0; + result.linear.y = 0.0; + result.linear.z = 0.0; + result.angular.x = 0.0; + result.angular.y = 0.0; + result.angular.z = 0.0; + return result; +} + +template +void set_vendor_stamp(Stamp& stamp, int32_t sec, uint32_t nsec) { + static_assert(requires(Stamp value) { value.sec = int32_t{}; }, + "M20 vendor Timestamp must expose sec"); + static_assert(requires(Stamp value) { value.nanosec = uint32_t{}; } || + requires(Stamp value) { value.nsec = uint32_t{}; }, + "M20 vendor Timestamp must expose nanosec or nsec"); + stamp.sec = sec; + if constexpr (requires { stamp.nanosec = nsec; }) { + stamp.nanosec = nsec; + } else { + stamp.nsec = nsec; + } +} + +// Released M20 message packages have used both `stamp` and `timestamp` in +// MetaType. Keep the bridge source-compatible with either installed version. +template +void set_vendor_header(Header& header, uint64_t frame_id, const rclcpp::Time& now) { + static_assert(requires(Header value) { value.frame_id = uint64_t{}; }, + "M20 vendor MetaType must expose frame_id"); + static_assert(requires(Header value) { value.timestamp; } || + requires(Header value) { value.stamp; }, + "M20 vendor MetaType must expose timestamp or stamp"); + header.frame_id = frame_id; + const int64_t total_ns = now.nanoseconds(); + const auto sec = static_cast(total_ns / kNanosecondsPerSecond); + const auto nsec = static_cast(total_ns % kNanosecondsPerSecond); + if constexpr (requires { header.timestamp; }) { + set_vendor_stamp(header.timestamp, sec, nsec); + } else { + set_vendor_stamp(header.stamp, sec, nsec); + } +} + +std_msgs::Header to_dimos_header(const std_msgs::msg::Header& source, + const std::string& frame_id) { + static std::atomic sequence{0}; + std_msgs::Header result; + result.seq = sequence.fetch_add(1, std::memory_order_relaxed); + result.stamp.sec = source.stamp.sec; + result.stamp.nsec = static_cast(source.stamp.nanosec); + result.frame_id = frame_id; + return result; +} + +} // namespace + +struct M20ROSBridgeConfig { + std::string lidar_topic; + std::string odom_topic; + std::string nav_cmd_topic; + std::string location_status_topic; + std::string hes_status_topic; + std::string node_name; + std::string cloud_frame; + std::string world_frame; + std::string base_frame; + bool enable_command_output; + double command_rate_hz; + double command_timeout_s; + double safety_timeout_s; + double max_linear_x; + double max_linear_y; + double max_angular_z; + + void validate() const { + require_nonempty(lidar_topic, "lidar_topic"); + require_nonempty(odom_topic, "odom_topic"); + require_nonempty(nav_cmd_topic, "nav_cmd_topic"); + require_nonempty(location_status_topic, "location_status_topic"); + require_nonempty(hes_status_topic, "hes_status_topic"); + require_nonempty(node_name, "node_name"); + require_nonempty(cloud_frame, "cloud_frame"); + require_nonempty(world_frame, "world_frame"); + require_nonempty(base_frame, "base_frame"); + dimos::native::require_positive(command_rate_hz, "command_rate_hz"); + dimos::native::require_positive(command_timeout_s, "command_timeout_s"); + dimos::native::require_positive(safety_timeout_s, "safety_timeout_s"); + dimos::native::require_positive(max_linear_x, "max_linear_x"); + dimos::native::require_positive(max_linear_y, "max_linear_y"); + dimos::native::require_positive(max_angular_z, "max_angular_z"); + } +}; + +class M20ROSBridge : public Module { +public: + void build(Builder& builder, Config& config) override { + cfg_ = config.parse(); + builder.input("safe_cmd_vel", &M20ROSBridge::on_command, this); + command_ready_ = builder.output("command_ready"); + lidar_ = builder.output("lidar"); + odom_ = builder.output("odom"); + odometry_ = builder.output("odometry"); + tf_ = builder.output("tf"); + } + + void setup() override { + rclcpp::init(0, nullptr); + // rclcpp installs process signal handlers during init. Restore the + // NativeModule handlers so coordinator SIGTERM exits Module::handle(); + // teardown below then cancels the executor and shuts rclcpp down. + dimos::native::install_signal_handlers(); + node_ = std::make_shared(cfg_.node_name); + + const auto sensor_qos = rclcpp::SensorDataQoS().keep_last(2); + lidar_subscription_ = node_->create_subscription( + cfg_.lidar_topic, sensor_qos, + [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { on_lidar(*msg); }); + odom_subscription_ = node_->create_subscription( + cfg_.odom_topic, sensor_qos, + [this](nav_msgs::msg::Odometry::SharedPtr msg) { on_odometry(*msg); }); + location_subscription_ = node_->create_subscription( + cfg_.location_status_topic, sensor_qos, + [this](drdds::msg::LocationStatus::SharedPtr msg) { on_location_status(*msg); }); + hes_subscription_ = node_->create_subscription( + cfg_.hes_status_topic, sensor_qos, + [this](drdds::msg::StdMsgInt32::SharedPtr msg) { on_hes_status(*msg); }); + + if (cfg_.enable_command_output) { + nav_cmd_publisher_ = node_->create_publisher( + cfg_.nav_cmd_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); + } + + const auto period = std::chrono::duration(1.0 / cfg_.command_rate_hz); + timer_ = node_->create_wall_timer( + std::chrono::duration_cast(period), + [this]() { publish_cycle(false); }); + + executor_ = std::make_shared(); + executor_->add_node(node_); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + + logging::info( + "M20 ROS bridge started", + {logging::Field("lidar_topic", cfg_.lidar_topic), + logging::Field("odom_topic", cfg_.odom_topic), + logging::Field("command_output", cfg_.enable_command_output)}); + } + + void teardown() override { + stopping_.store(true, std::memory_order_release); + timer_.reset(); + if (nav_cmd_publisher_ != nullptr && rclcpp::ok()) { + publish_cycle(true); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + publish_cycle(true); + } + if (executor_ != nullptr) { + executor_->cancel(); + } + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + nav_cmd_publisher_.reset(); + hes_subscription_.reset(); + location_subscription_.reset(); + odom_subscription_.reset(); + lidar_subscription_.reset(); + if (executor_ != nullptr && node_ != nullptr) { + executor_->remove_node(node_); + } + node_.reset(); + executor_.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } + +private: + void on_lidar(const sensor_msgs::msg::PointCloud2& source) { + try { + sensor_msgs::PointCloud2 result; + result.header = to_dimos_header(source.header, cfg_.cloud_frame); + result.height = checked_i32(source.height, "point-cloud height"); + result.width = checked_i32(source.width, "point-cloud width"); + result.fields_length = checked_i32(source.fields.size(), "point-cloud field count"); + result.fields.reserve(source.fields.size()); + for (const auto& source_field : source.fields) { + sensor_msgs::PointField field; + field.name = source_field.name; + field.offset = checked_i32(source_field.offset, "point-field offset"); + field.datatype = source_field.datatype; + field.count = checked_i32(source_field.count, "point-field count"); + result.fields.push_back(std::move(field)); + } + result.is_bigendian = static_cast(source.is_bigendian); + result.point_step = checked_i32(source.point_step, "point-cloud point step"); + result.row_step = checked_i32(source.row_step, "point-cloud row step"); + result.data_length = checked_i32(source.data.size(), "point-cloud byte count"); + result.data = source.data; + result.is_dense = static_cast(source.is_dense); + lidar_.publish(result); + } catch (const std::exception& error) { + logging::error("dropping invalid M20 point cloud", + {logging::Field("error", std::string(error.what()))}); + } + } + + void on_odometry(const nav_msgs::msg::Odometry& source) { + nav_msgs::Odometry result; + result.header = to_dimos_header(source.header, cfg_.world_frame); + result.child_frame_id = cfg_.base_frame; + + result.pose.pose.position.x = source.pose.pose.position.x; + result.pose.pose.position.y = source.pose.pose.position.y; + result.pose.pose.position.z = source.pose.pose.position.z; + result.pose.pose.orientation.x = source.pose.pose.orientation.x; + result.pose.pose.orientation.y = source.pose.pose.orientation.y; + result.pose.pose.orientation.z = source.pose.pose.orientation.z; + result.pose.pose.orientation.w = source.pose.pose.orientation.w; + result.twist.twist.linear.x = source.twist.twist.linear.x; + result.twist.twist.linear.y = source.twist.twist.linear.y; + result.twist.twist.linear.z = source.twist.twist.linear.z; + result.twist.twist.angular.x = source.twist.twist.angular.x; + result.twist.twist.angular.y = source.twist.twist.angular.y; + result.twist.twist.angular.z = source.twist.twist.angular.z; + for (std::size_t i = 0; i < source.pose.covariance.size(); ++i) { + result.pose.covariance[i] = source.pose.covariance[i]; + result.twist.covariance[i] = source.twist.covariance[i]; + } + + geometry_msgs::PoseStamped pose; + pose.header = result.header; + pose.pose = result.pose.pose; + + geometry_msgs::TransformStamped transform; + transform.header = result.header; + transform.child_frame_id = cfg_.base_frame; + transform.transform.translation.x = source.pose.pose.position.x; + transform.transform.translation.y = source.pose.pose.position.y; + transform.transform.translation.z = source.pose.pose.position.z; + transform.transform.rotation = result.pose.pose.orientation; + tf2_msgs::TFMessage transforms; + transforms.transforms_length = 1; + transforms.transforms.push_back(std::move(transform)); + + odometry_.publish(result); + odom_.publish(pose); + tf_.publish(transforms); + } + + void on_command(const geometry_msgs::Twist& source) { + geometry_msgs::Twist bounded = zero_twist(); + if (std::isfinite(source.linear.x) && std::isfinite(source.linear.y) && + std::isfinite(source.angular.z)) { + bounded.linear.x = clamp(source.linear.x, cfg_.max_linear_x); + bounded.linear.y = clamp(source.linear.y, cfg_.max_linear_y); + bounded.angular.z = clamp(source.angular.z, cfg_.max_angular_z); + } + std::lock_guard lock(state_mutex_); + latest_command_ = bounded; + command_received_at_ = Clock::now(); + have_command_ = true; + } + + void on_location_status(const drdds::msg::LocationStatus& source) { + std::lock_guard lock(state_mutex_); + location_status_ = static_cast(source.data.total_status); + location_received_at_ = Clock::now(); + have_location_ = true; + } + + void on_hes_status(const drdds::msg::StdMsgInt32& source) { + std::lock_guard lock(state_mutex_); + hes_status_ = static_cast(source.data); + hes_received_at_ = Clock::now(); + have_hes_ = true; + } + + bool safety_ready(Clock::time_point now) const { + std::lock_guard lock(state_mutex_); + if (!cfg_.enable_command_output || !have_location_ || !have_hes_) { + return false; + } + const auto timeout = std::chrono::duration(cfg_.safety_timeout_s); + return now - location_received_at_ <= timeout && now - hes_received_at_ <= timeout && + location_status_ == 1 && hes_status_ == 0; + } + + geometry_msgs::Twist fresh_command_or_zero(Clock::time_point now) const { + std::lock_guard lock(state_mutex_); + const auto timeout = std::chrono::duration(cfg_.command_timeout_s); + if (!have_command_ || now - command_received_at_ > timeout) { + return zero_twist(); + } + return latest_command_; + } + + void publish_cycle(bool force_zero) { + const auto now = Clock::now(); + const bool ready = !force_zero && !stopping_.load(std::memory_order_acquire) && + nav_cmd_publisher_ != nullptr && + nav_cmd_publisher_->get_subscription_count() > 0 && + safety_ready(now); + std_msgs::Bool ready_message; + ready_message.data = static_cast(ready); + command_ready_.publish(ready_message); + + if (nav_cmd_publisher_ == nullptr) { + return; + } + const geometry_msgs::Twist command = ready ? fresh_command_or_zero(now) : zero_twist(); + drdds::msg::NavCmd output; + set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); + output.data.x_vel = static_cast(command.linear.x); + output.data.y_vel = static_cast(command.linear.y); + output.data.yaw_vel = static_cast(command.angular.z); + nav_cmd_publisher_->publish(output); + } + + M20ROSBridgeConfig cfg_; + Output command_ready_; + Output lidar_; + Output odom_; + Output odometry_; + Output tf_; + + std::shared_ptr node_; + std::shared_ptr executor_; + rclcpp::Subscription::SharedPtr lidar_subscription_; + rclcpp::Subscription::SharedPtr odom_subscription_; + rclcpp::Subscription::SharedPtr location_subscription_; + rclcpp::Subscription::SharedPtr hes_subscription_; + rclcpp::Publisher::SharedPtr nav_cmd_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + std::thread spin_thread_; + + mutable std::mutex state_mutex_; + geometry_msgs::Twist latest_command_ = zero_twist(); + Clock::time_point command_received_at_{}; + Clock::time_point location_received_at_{}; + Clock::time_point hes_received_at_{}; + bool have_command_ = false; + bool have_location_ = false; + bool have_hes_ = false; + int location_status_ = 0; + int hes_status_ = 1; + std::atomic stopping_{false}; + std::atomic command_sequence_{0}; +}; + +int main() { + dimos::native::run_with_transport(); + return 0; +} diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py new file mode 100644 index 0000000000..6e40151915 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -0,0 +1,85 @@ +# 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. + +"""NativeModule declaration for the M20's local ROS 2/DrDDS adapter.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from pydantic import Field + +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.robot.deeprobotics.m20.constants import ( + MAX_ANGULAR_Z_RAD_S, + MAX_LINEAR_X_M_S, + MAX_LINEAR_Y_M_S, +) +from dimos.spec import perception + + +class M20ROSBridgeConfig(NativeModuleConfig): + """Robot-local ROS topic names, frames, and command watchdog settings.""" + + cwd: str | None = "cpp" + executable: str = "build/m20_ros_bridge" + build_command: str | None = "./build.sh" + stdin_config: bool = True + + lidar_topic: str = "/LIDAR/POINTS" + odom_topic: str = "/ODOM" + nav_cmd_topic: str = "/NAV_CMD" + location_status_topic: str = "/LOCATION_STATUS" + hes_status_topic: str = "/HES_STATUS" + node_name: str = "dimos_m20_bridge" + + cloud_frame: str = "base_link" + world_frame: str = "odom" + base_frame: str = "base_link" + + enable_command_output: bool = False + command_rate_hz: float = Field(default=10.0, gt=0.0) + command_timeout_s: float = Field(default=0.4, gt=0.0) + safety_timeout_s: float = Field(default=2.5, gt=0.0) + max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) + max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) + max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) + + +class M20ROSBridge(NativeModule, perception.Lidar, perception.Odometry): + """Bridge M20 ROS 2 topics to local typed DimOS streams. + + This process runs on GOS and links against the robot's installed Foxy and + ``drdds`` packages. It does not route through Zenoh or another robot host. + """ + + config: M20ROSBridgeConfig + + safe_cmd_vel: In[Twist] + command_ready: Out[Bool] + lidar: Out[PointCloud2] + odom: Out[PoseStamped] + odometry: Out[Odometry] + tf: Out[TFMessage] + + +if TYPE_CHECKING: + M20ROSBridge() diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py new file mode 100644 index 0000000000..1e937c2b06 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -0,0 +1,171 @@ +# 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. + +"""Guarded high-level velocity connection for the Deep Robotics Lynx M20.""" + +from __future__ import annotations + +import math +from threading import RLock +from typing import Any + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.core.core import rpc +from dimos.core.module import Module, ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.robot.deeprobotics.m20.constants import ( + MAX_ANGULAR_Z_RAD_S, + MAX_LINEAR_X_M_S, + MAX_LINEAR_Y_M_S, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +class M20ConnectionConfig(ModuleConfig): + """Limits for commands sent to the M20's high-level navigation interface.""" + + max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) + max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) + max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) + require_command_ready: bool = True + + +def _clamp(value: float, limit: float) -> float: + return max(-limit, min(limit, value)) + + +def sanitize_twist(twist: Twist, config: M20ConnectionConfig) -> Twist: + """Return a finite, planar Twist bounded by the configured M20 limits.""" + values = (twist.linear.x, twist.linear.y, twist.angular.z) + if not all(math.isfinite(value) for value in values): + return Twist.zero() + return Twist( + linear=Vector3( + _clamp(twist.linear.x, config.max_linear_x), + _clamp(twist.linear.y, config.max_linear_y), + 0.0, + ), + angular=Vector3(0.0, 0.0, _clamp(twist.angular.z, config.max_angular_z)), + ) + + +class M20Connection(Module): + """Expose the planner-facing M20 command surface with an explicit safety gate. + + The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module + remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, + rejects it while disarmed, bounds planar commands while armed, and emits + ``safe_cmd_vel`` for the robot-local bridge. + + Arming never changes robot motion state, gait, vendor services, or charging + state. Those remain explicit deployment/operator responsibilities. + """ + + config: M20ConnectionConfig + + cmd_vel: In[Twist] + command_ready: In[Bool] + safe_cmd_vel: Out[Twist] + armed: Out[Bool] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._lock = RLock() + self._armed = False + self._command_ready = False + + @rpc + def start(self) -> None: + super().start() + self.register_disposable(Disposable(self.command_ready.subscribe(self._on_command_ready))) + self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) + self.safe_cmd_vel.publish(Twist.zero()) + self.armed.publish(Bool(False)) + + @rpc + def stop(self) -> None: + self.disarm() + super().stop() + + @rpc + def arm(self) -> bool: + """Allow bounded planner commands to reach the M20 ROS bridge.""" + with self._lock: + if self.config.require_command_ready and not self._command_ready: + logger.warning("M20 command gate refused arm: native bridge is not ready") + return False + self._armed = True + self.armed.publish(Bool(True)) + logger.warning("M20 command gate armed") + return True + + @rpc + def disarm(self) -> bool: + """Block commands and publish an immediate zero velocity.""" + with self._lock: + self._armed = False + self.safe_cmd_vel.publish(Twist.zero()) + self.armed.publish(Bool(False)) + logger.info("M20 command gate disarmed") + return True + + @rpc + def is_armed(self) -> bool: + """Return whether nonzero commands may pass through the gate.""" + with self._lock: + return self._armed + + @rpc + def is_command_ready(self) -> bool: + """Return whether the native bridge reports a fresh safe command path.""" + with self._lock: + return self._command_ready + + @rpc + def move(self, twist: Twist, duration: float = 0.0) -> bool: + """Forward a bounded planar velocity when armed. + + ``duration`` is accepted for connection compatibility. Command lifetime + is enforced by the native bridge's monotonic watchdog. + """ + del duration + with self._lock: + enabled = self._armed and (self._command_ready or not self.config.require_command_ready) + command = sanitize_twist(twist, self.config) if enabled else Twist.zero() + self.safe_cmd_vel.publish(command) + return enabled + + @rpc + def stop_movement(self) -> None: + """Publish an immediate zero velocity without changing the arm state.""" + self.safe_cmd_vel.publish(Twist.zero()) + + def _on_command_ready(self, msg: Bool) -> None: + ready = bool(msg.data) + with self._lock: + was_armed = self._armed + self._command_ready = ready + if not ready: + self._armed = False + if was_armed and not ready: + self.safe_cmd_vel.publish(Twist.zero()) + self.armed.publish(Bool(False)) + logger.warning("M20 command gate disarmed: native bridge lost readiness") diff --git a/dimos/robot/deeprobotics/m20/constants.py b/dimos/robot/deeprobotics/m20/constants.py new file mode 100644 index 0000000000..cbfa1feed9 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/constants.py @@ -0,0 +1,37 @@ +# 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. + +"""Documented and provisional geometry values for the Lynx M20.""" + +import math + +# Documented body dimensions in the vendor hardware guide. +BODY_LENGTH_M = 0.82 +BODY_WIDTH_M = 0.506 + +# The official M20 locomotion SDK's stand_height_ default. Verify that the +# vendor /ODOM child pose uses the same base origin before hardware tuning. +BASE_LINK_HEIGHT_M = 0.48 + +# Conservative initial MLS clearance. This includes the body above base_link +# and remains a hardware-tuning value rather than a vendor specification. +PLANNING_HEIGHT_M = 0.65 + +ROTATION_DIAMETER_M = math.hypot(BODY_LENGTH_M, BODY_WIDTH_M) + +# Conservative command bounds until direction signs, gait, latency, and +# stopping distance have been measured on the actual robot. +MAX_LINEAR_X_M_S = 0.3 +MAX_LINEAR_Y_M_S = 0.3 +MAX_ANGULAR_Z_RAD_S = 0.5 diff --git a/dimos/robot/deeprobotics/m20/test_connection.py b/dimos/robot/deeprobotics/m20/test_connection.py new file mode 100644 index 0000000000..0bb560aba0 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/test_connection.py @@ -0,0 +1,173 @@ +# 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. + +"""Behavior tests for the guarded M20 command surface.""" + +from collections.abc import Callable, Iterator +import math + +import pytest +from pytest_mock import MockerFixture + +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.protocol.rpc.pubsubrpc import LCMRPC +from dimos.robot.deeprobotics.m20.connection import ( + M20Connection, + M20ConnectionConfig, + sanitize_twist, +) + + +@pytest.fixture +def connection_factory( + mocker: MockerFixture, +) -> Iterator[Callable[..., M20Connection]]: + """Create connections and close their transport resources after each test.""" + del mocker # Keep mocked methods installed until connection.stop() completes. + connections: list[M20Connection] = [] + + def create(**kwargs: float) -> M20Connection: + connection = M20Connection(rpc_transport=LCMRPC, **kwargs) + connections.append(connection) + return connection + + yield create + + for connection in connections: + connection.stop() + + +def test_sanitize_twist_bounds_planar_command() -> None: + config = M20ConnectionConfig( + max_linear_x=0.5, + max_linear_y=0.25, + max_angular_z=0.75, + ) + command = Twist( + linear=Vector3(2.0, -2.0, 4.0), + angular=Vector3(1.0, 2.0, -3.0), + ) + + result = sanitize_twist(command, config) + + assert result == Twist( + linear=Vector3(0.5, -0.25, 0.0), + angular=Vector3(0.0, 0.0, -0.75), + ) + + +def test_sanitize_twist_rejects_nonfinite_command() -> None: + config = M20ConnectionConfig() + command = Twist(linear=Vector3(math.nan, 0.0, 0.0)) + + result = sanitize_twist(command, config) + + assert result == Twist.zero() + + +def test_connection_blocks_commands_until_explicitly_armed( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + command = Twist(linear=Vector3(0.2, 0.0, 0.0)) + + accepted = connection.move(command) + + assert accepted is False + publish.assert_called_once_with(Twist.zero()) + + +def test_connection_forwards_bounded_command_after_arm( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory( + max_linear_x=0.4, + max_linear_y=0.3, + max_angular_z=0.6, + ) + safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + armed_publish = mocker.patch.object(connection.armed, "publish") + command = Twist(linear=Vector3(0.7, -0.4, 2.0), angular=Vector3(1.0, 2.0, 0.9)) + + connection._on_command_ready(Bool(True)) + connection.arm() + accepted = connection.move(command) + + assert accepted is True + armed_publish.assert_called_once() + assert armed_publish.call_args.args[0].data is True + safe_publish.assert_called_once_with( + Twist(linear=Vector3(0.4, -0.3, 0.0), angular=Vector3(0.0, 0.0, 0.6)) + ) + + +def test_disarm_publishes_zero_and_blocks_following_commands( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + armed_publish = mocker.patch.object(connection.armed, "publish") + + connection._on_command_ready(Bool(True)) + connection.arm() + safe_publish.reset_mock() + armed_publish.reset_mock() + connection.disarm() + accepted = connection.move(Twist(linear=Vector3(0.2, 0.0, 0.0))) + + assert accepted is False + assert safe_publish.call_args_list == [mocker.call(Twist.zero()), mocker.call(Twist.zero())] + armed_publish.assert_called_once() + assert armed_publish.call_args.args[0].data is False + + +def test_connection_refuses_arm_until_native_bridge_is_ready( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + armed_publish = mocker.patch.object(connection.armed, "publish") + + accepted = connection.arm() + + assert accepted is False + assert connection.is_armed() is False + armed_publish.assert_not_called() + + +def test_connection_disarms_when_native_bridge_loses_readiness( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_command_ready(Bool(True)) + connection.arm() + safe_publish.reset_mock() + armed_publish.reset_mock() + + connection._on_command_ready(Bool(False)) + + assert connection.is_command_ready() is False + assert connection.is_armed() is False + safe_publish.assert_called_once_with(Twist.zero()) + armed_publish.assert_called_once() + assert armed_publish.call_args.args[0].data is False From 765cfd019a266f39714ea6d566bb314c74ce5a90 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Fri, 28 Aug 2026 20:45:47 +0800 Subject: [PATCH 02/15] fix(robot): supervise M20 lidar streaming --- dimos/mapping/ray_tracing/rust/src/module.rs | 48 ++++ .../m20/blueprints/test_m20_kronknav.py | 3 + dimos/robot/deeprobotics/m20/bridge/README.md | 35 ++- .../m20/bridge/cpp/CMakeLists.txt | 2 +- .../deeprobotics/m20/bridge/cpp/main.cpp | 248 +++++++++++++++++- dimos/robot/deeprobotics/m20/bridge/module.py | 2 + .../deeprobotics/m20/bridge/test_module.py | 37 +++ dimos/robot/deeprobotics/m20/connection.py | 32 ++- dimos/robot/deeprobotics/m20/deploy/README.md | 118 +++++++++ .../deploy/dimos-m20-rsdriver-shm-permissions | 36 +++ .../deeprobotics/m20/deploy/dimos-m20.env | 3 + .../10-dimos-shm-permissions.conf | 8 + .../robot/deeprobotics/m20/test_connection.py | 52 ++++ native/cpp/include/dimos/native/config.hpp | 34 ++- native/cpp/tests/test_config.cpp | 21 ++ 15 files changed, 647 insertions(+), 32 deletions(-) create mode 100644 dimos/robot/deeprobotics/m20/bridge/test_module.py create mode 100644 dimos/robot/deeprobotics/m20/deploy/README.md create mode 100755 dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions create mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20.env create mode 100644 dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf diff --git a/dimos/mapping/ray_tracing/rust/src/module.rs b/dimos/mapping/ray_tracing/rust/src/module.rs index 674bc2ae9f..ca601944a1 100644 --- a/dimos/mapping/ray_tracing/rust/src/module.rs +++ b/dimos/mapping/ray_tracing/rust/src/module.rs @@ -411,6 +411,54 @@ mod tests { ) } + #[test] + fn extract_xyz_accepts_the_m20_extended_point_layout() { + let field = |name: &str, offset: i32, datatype: u8| PointField { + name: name.into(), + offset, + datatype, + count: 1, + }; + let expected = [(1.25_f32, -2.5_f32, 0.75_f32), (-4.0_f32, 5.5_f32, 1.0_f32)]; + let mut data = vec![0_u8; expected.len() * 26]; + for (index, &(x, y, z)) in expected.iter().enumerate() { + let base = index * 26; + data[base..base + 4].copy_from_slice(&x.to_le_bytes()); + data[base + 4..base + 8].copy_from_slice(&y.to_le_bytes()); + data[base + 8..base + 12].copy_from_slice(&z.to_le_bytes()); + data[base + 12..base + 16].copy_from_slice(&42.0_f32.to_le_bytes()); + data[base + 16..base + 18].copy_from_slice(&7_u16.to_le_bytes()); + data[base + 18..base + 26].copy_from_slice(&1_718_663_385.5_f64.to_le_bytes()); + } + let cloud = PointCloud2 { + header: Header { + frame_id: "base_link".into(), + ..Header::default() + }, + height: 1, + width: expected.len() as i32, + fields: vec![ + field("x", 0, PointField::FLOAT32 as u8), + field("y", 4, PointField::FLOAT32 as u8), + field("z", 8, PointField::FLOAT32 as u8), + field("intensity", 12, PointField::FLOAT32 as u8), + field("ring", 16, PointField::UINT16 as u8), + field("timestamp", 18, PointField::FLOAT64 as u8), + ], + is_bigendian: false, + point_step: 26, + row_step: expected.len() as i32 * 26, + data, + is_dense: false, + }; + + let Ok(decoded) = extract_xyz(&cloud) else { + panic!("M20 clouds must be mapper-compatible"); + }; + + assert_eq!(decoded, expected); + } + /// The clear-mask handler names voxels by decoding a cloud and quantizing /// it. Both halves have to agree with how returns were quantized on the way /// in, or a mask silently clears nothing. diff --git a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py index 10ef55b732..be89ac90b6 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py @@ -20,6 +20,7 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.dannav.local_planner.module import DanLocalPlanner @@ -76,6 +77,8 @@ def test_sensor_and_pose_streams_reach_mapping_and_navigation() -> None: assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20ROSBridge} assert RayTracingVoxelMap in _endpoint_modules(blueprint, "lidar", PointCloud2, "in") + assert _endpoint_modules(blueprint, "lidar_ready", Bool, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "lidar_ready", Bool, "in") == {M20Connection} assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20ROSBridge} assert _endpoint_modules(blueprint, "tf", TFMessage, "in") == { RayTracingVoxelMap, diff --git a/dimos/robot/deeprobotics/m20/bridge/README.md b/dimos/robot/deeprobotics/m20/bridge/README.md index 2b12ab3386..eca3f203f5 100644 --- a/dimos/robot/deeprobotics/m20/bridge/README.md +++ b/dimos/robot/deeprobotics/m20/bridge/README.md @@ -9,21 +9,50 @@ blueprints therefore pin the complete onboard graph to LCM; this bridge does not contain a private Zenoh implementation. The bridge always subscribes to `/LIDAR/POINTS`, `/ODOM`, `/LOCATION_STATUS`, -and `/HES_STATUS`. It publishes lidar, pose, odometry, TF, and command-readiness -streams into the local DimOS graph. +and `/HES_STATUS`. It publishes lidar, pose, odometry, TF, lidar-readiness, and +command-readiness streams into the local DimOS graph. + +The inspected M20 publishes merged front/rear clouds at 10 Hz with reliable, +volatile DDS QoS. Each point uses the vendor's 26-byte layout (`x`, `y`, `z`, +`intensity`, `ring`, `timestamp`); the bridge preserves the fields and bytes. +Before forwarding, it rejects empty, malformed, big-endian, or XYZ-less clouds. +The native steady-clock watchdog marks the stream stale after 0.5 seconds +without a mapper-compatible cloud and logs every loss/recovery transition. The +vendor driver reports `lidar_link`, although its documented merge already +applies both sensor extrinsics into `base_link`, so the bridge normalizes that +known-mislabeled frame to `base_link`. + +GOS starts `rsdriver.service` as root and creates its Fast DDS shared-memory +segment with mode `0644`. A normal `user` subscriber cannot attach, and the +vendor writer does not fall back to UDP for a same-host cloud reader. Install +the checked-in `deploy/rsdriver.service.d` drop-in and its permission helper. +The service stays root for real-time scheduling, but its active Fast DDS files +become group-writable by the existing `user` group so DimOS remains unprivileged. `enable_command_output` defaults to `false`. When explicitly enabled, the bridge owns a `/NAV_CMD` publisher but emits nonzero velocity only while: - location status is fresh and exactly `1` (normal); - hard-estop status is fresh and exactly `0` (not triggered); +- a valid merged lidar cloud has arrived within the lidar timeout; - the `/NAV_CMD` publisher has a matched subscriber; - the Python connection has explicitly armed and supplied a fresh bounded command. The native watchdog uses a steady clock and sends zero after command timeout, -on health loss, and during shutdown. Starting the bridge never changes robot +on lidar or robot-health loss, and during shutdown. Starting the bridge never changes robot mode, gait, planner service, charging state, or standing state. +The bridge diagnoses but does not remotely manage the vendor sensor pipeline. +For boot-persistent clouds, `multicast-relay.service` must be enabled on NOS and +`rsdriver.service` enabled on GOS; both must be active before DimOS starts. +Install the GOS service drop-in/helper from `deploy/`, and load +`deploy/dimos-m20.env` in the DimOS launcher. The explicit 16 MiB LCM receive +buffer is required for the observed 0.8-2.1 MB clouds; the LCM bus keeps its +default TTL 0, so this high-bandwidth stream remains local to GOS. + +See [`deploy/README.md`](../deploy/README.md) for the complete packet path, +persistent installation steps, live checks, and remote Rerun attachment. + Build on GOS after sourcing the vendor environment: ```bash diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt index 1f3f4fc9fb..23a3611546 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt @@ -47,7 +47,7 @@ ament_target_dependencies(m20_ros_bridge rclcpp sensor_msgs ) -target_link_libraries(m20_ros_bridge PRIVATE +target_link_libraries(m20_ros_bridge dimos_native PkgConfig::LCM ) diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp index 7bc44a63b4..4aa24b08fa 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp @@ -10,12 +10,14 @@ #include #include #include +#include #include #include #include #include #include #include +#include #include #include @@ -24,6 +26,7 @@ #include #include #include +#include #include "dimos/native.hpp" @@ -61,6 +64,91 @@ int32_t checked_i32(std::size_t value, const char* name) { return static_cast(value); } +std::size_t checked_product(std::size_t left, std::size_t right, const char* name) { + if (left != 0 && right > std::numeric_limits::max() / left) { + throw std::runtime_error(std::string(name) + " overflows size_t"); + } + return left * right; +} + +struct XYZOffsets { + std::size_t x; + std::size_t y; + std::size_t z; +}; + +XYZOffsets validate_cloud_for_mapping(const sensor_msgs::msg::PointCloud2& cloud) { + if (cloud.width == 0 || cloud.height == 0) { + throw std::runtime_error("point cloud is empty"); + } + if (cloud.is_bigendian) { + throw std::runtime_error("big-endian point clouds are not supported by the mapper"); + } + if (cloud.point_step == 0) { + throw std::runtime_error("point-cloud point_step is zero"); + } + if (cloud.point_step < sizeof(float)) { + throw std::runtime_error("point-cloud point_step is shorter than float32"); + } + + const auto row_bytes = checked_product(static_cast(cloud.width), + static_cast(cloud.point_step), + "point-cloud row size"); + if (cloud.row_step != row_bytes) { + throw std::runtime_error("point cloud contains unsupported row padding"); + } + const auto required_bytes = checked_product(row_bytes, static_cast(cloud.height), + "point-cloud byte count"); + if (cloud.data.size() < required_bytes) { + throw std::runtime_error("point-cloud data is shorter than its dimensions"); + } + + XYZOffsets offsets{std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + for (const auto& field : cloud.fields) { + if (field.datatype != sensor_msgs::msg::PointField::FLOAT32 || field.count == 0) { + continue; + } + const auto offset = static_cast(field.offset); + if (offset > static_cast(cloud.point_step) - sizeof(float)) { + continue; + } + if (field.name == "x") { + offsets.x = offset; + } else if (field.name == "y") { + offsets.y = offset; + } else if (field.name == "z") { + offsets.z = offset; + } + } + const auto missing = std::numeric_limits::max(); + if (offsets.x == missing || offsets.y == missing || offsets.z == missing) { + throw std::runtime_error("point cloud lacks mapper-compatible float32 x/y/z fields"); + } + return offsets; +} + +bool has_finite_xyz(const sensor_msgs::msg::PointCloud2& cloud, const XYZOffsets& offsets) { + const auto point_count = checked_product(static_cast(cloud.width), + static_cast(cloud.height), + "point-cloud point count"); + const auto point_step = static_cast(cloud.point_step); + for (std::size_t index = 0; index < point_count; ++index) { + const auto base = index * point_step; + float x = 0.0F; + float y = 0.0F; + float z = 0.0F; + std::memcpy(&x, cloud.data.data() + base + offsets.x, sizeof(float)); + std::memcpy(&y, cloud.data.data() + base + offsets.y, sizeof(float)); + std::memcpy(&z, cloud.data.data() + base + offsets.z, sizeof(float)); + if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { + return true; + } + } + return false; +} + double clamp(double value, double limit) { return std::max(-limit, std::min(limit, value)); } @@ -76,15 +164,54 @@ geometry_msgs::Twist zero_twist() { return result; } +template +struct HasSec : std::false_type {}; +template +struct HasSec().sec)>> : std::true_type {}; + +template +struct HasNanosec : std::false_type {}; +template +struct HasNanosec().nanosec)>> : std::true_type {}; + +template +struct HasNsec : std::false_type {}; +template +struct HasNsec().nsec)>> : std::true_type {}; + +template +struct HasFrameId : std::false_type {}; +template +struct HasFrameId().frame_id)>> : std::true_type {}; + +template +struct HasTimestamp : std::false_type {}; +template +struct HasTimestamp().timestamp)>> + : std::true_type {}; + +template +struct HasStamp : std::false_type {}; +template +struct HasStamp().stamp)>> : std::true_type {}; + +template +struct HasValue : std::false_type {}; +template +struct HasValue().value)>> : std::true_type {}; + +template +struct HasData : std::false_type {}; +template +struct HasData().data)>> : std::true_type {}; + template void set_vendor_stamp(Stamp& stamp, int32_t sec, uint32_t nsec) { - static_assert(requires(Stamp value) { value.sec = int32_t{}; }, - "M20 vendor Timestamp must expose sec"); - static_assert(requires(Stamp value) { value.nanosec = uint32_t{}; } || - requires(Stamp value) { value.nsec = uint32_t{}; }, + static_assert(HasSec::value, "M20 vendor Timestamp must expose sec"); + static_assert(HasNanosec::value || HasNsec::value, "M20 vendor Timestamp must expose nanosec or nsec"); stamp.sec = sec; - if constexpr (requires { stamp.nanosec = nsec; }) { + if constexpr (HasNanosec::value) { stamp.nanosec = nsec; } else { stamp.nsec = nsec; @@ -95,22 +222,31 @@ void set_vendor_stamp(Stamp& stamp, int32_t sec, uint32_t nsec) { // MetaType. Keep the bridge source-compatible with either installed version. template void set_vendor_header(Header& header, uint64_t frame_id, const rclcpp::Time& now) { - static_assert(requires(Header value) { value.frame_id = uint64_t{}; }, - "M20 vendor MetaType must expose frame_id"); - static_assert(requires(Header value) { value.timestamp; } || - requires(Header value) { value.stamp; }, + static_assert(HasFrameId
::value, "M20 vendor MetaType must expose frame_id"); + static_assert(HasTimestamp
::value || HasStamp
::value, "M20 vendor MetaType must expose timestamp or stamp"); header.frame_id = frame_id; const int64_t total_ns = now.nanoseconds(); const auto sec = static_cast(total_ns / kNanosecondsPerSecond); const auto nsec = static_cast(total_ns % kNanosecondsPerSecond); - if constexpr (requires { header.timestamp; }) { + if constexpr (HasTimestamp
::value) { set_vendor_stamp(header.timestamp, sec, nsec); } else { set_vendor_stamp(header.stamp, sec, nsec); } } +template +int vendor_int32_value(const Status& status) { + static_assert(HasValue::value || HasData::value, + "M20 vendor int32 status must expose value or data"); + if constexpr (HasValue::value) { + return static_cast(status.value); + } else { + return static_cast(status.data); + } +} + std_msgs::Header to_dimos_header(const std_msgs::msg::Header& source, const std::string& frame_id) { static std::atomic sequence{0}; @@ -138,6 +274,7 @@ struct M20ROSBridgeConfig { double command_rate_hz; double command_timeout_s; double safety_timeout_s; + double lidar_timeout_s; double max_linear_x; double max_linear_y; double max_angular_z; @@ -155,18 +292,44 @@ struct M20ROSBridgeConfig { dimos::native::require_positive(command_rate_hz, "command_rate_hz"); dimos::native::require_positive(command_timeout_s, "command_timeout_s"); dimos::native::require_positive(safety_timeout_s, "safety_timeout_s"); + dimos::native::require_positive(lidar_timeout_s, "lidar_timeout_s"); dimos::native::require_positive(max_linear_x, "max_linear_x"); dimos::native::require_positive(max_linear_y, "max_linear_y"); dimos::native::require_positive(max_angular_z, "max_angular_z"); } }; +M20ROSBridgeConfig parse_m20_config(Config& config) { + M20ROSBridgeConfig result{}; + result.lidar_topic = config.take("lidar_topic"); + result.odom_topic = config.take("odom_topic"); + result.nav_cmd_topic = config.take("nav_cmd_topic"); + result.location_status_topic = config.take("location_status_topic"); + result.hes_status_topic = config.take("hes_status_topic"); + result.node_name = config.take("node_name"); + result.cloud_frame = config.take("cloud_frame"); + result.world_frame = config.take("world_frame"); + result.base_frame = config.take("base_frame"); + result.enable_command_output = config.take("enable_command_output"); + result.command_rate_hz = config.take("command_rate_hz"); + result.command_timeout_s = config.take("command_timeout_s"); + result.safety_timeout_s = config.take("safety_timeout_s"); + result.lidar_timeout_s = config.take("lidar_timeout_s"); + result.max_linear_x = config.take("max_linear_x"); + result.max_linear_y = config.take("max_linear_y"); + result.max_angular_z = config.take("max_angular_z"); + config.enforce_all_consumed(); + result.validate(); + return result; +} + class M20ROSBridge : public Module { public: void build(Builder& builder, Config& config) override { - cfg_ = config.parse(); + cfg_ = parse_m20_config(config); builder.input("safe_cmd_vel", &M20ROSBridge::on_command, this); command_ready_ = builder.output("command_ready"); + lidar_ready_ = builder.output("lidar_ready"); lidar_ = builder.output("lidar"); odom_ = builder.output("odom"); odometry_ = builder.output("odometry"); @@ -181,9 +344,14 @@ class M20ROSBridge : public Module { dimos::native::install_signal_handlers(); node_ = std::make_shared(cfg_.node_name); + // Both inspected bare-DDS M20 publishers offer RELIABLE/VOLATILE. Pin + // the cloud subscription to that verified contract so DDS detects a + // future incompatible vendor QoS change instead of silently dropping. + const auto lidar_qos = + rclcpp::QoS(rclcpp::KeepLast(2)).reliable().durability_volatile(); const auto sensor_qos = rclcpp::SensorDataQoS().keep_last(2); lidar_subscription_ = node_->create_subscription( - cfg_.lidar_topic, sensor_qos, + cfg_.lidar_topic, lidar_qos, [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { on_lidar(*msg); }); odom_subscription_ = node_->create_subscription( cfg_.odom_topic, sensor_qos, @@ -248,6 +416,10 @@ class M20ROSBridge : public Module { private: void on_lidar(const sensor_msgs::msg::PointCloud2& source) { try { + const XYZOffsets offsets = validate_cloud_for_mapping(source); + if (!has_finite_xyz(source, offsets)) { + throw std::runtime_error("point cloud contains no finite XYZ return"); + } sensor_msgs::PointCloud2 result; result.header = to_dimos_header(source.header, cfg_.cloud_frame); result.height = checked_i32(source.height, "point-cloud height"); @@ -269,6 +441,12 @@ class M20ROSBridge : public Module { result.data = source.data; result.is_dense = static_cast(source.is_dense); lidar_.publish(result); + { + std::lock_guard lock(state_mutex_); + lidar_received_at_ = Clock::now(); + last_lidar_width_ = source.width; + have_lidar_ = true; + } } catch (const std::exception& error) { logging::error("dropping invalid M20 point cloud", {logging::Field("error", std::string(error.what()))}); @@ -341,7 +519,7 @@ class M20ROSBridge : public Module { void on_hes_status(const drdds::msg::StdMsgInt32& source) { std::lock_guard lock(state_mutex_); - hes_status_ = static_cast(source.data); + hes_status_ = vendor_int32_value(source); hes_received_at_ = Clock::now(); have_hes_ = true; } @@ -356,6 +534,21 @@ class M20ROSBridge : public Module { location_status_ == 1 && hes_status_ == 0; } + bool lidar_fresh(Clock::time_point now) const { + std::lock_guard lock(state_mutex_); + const auto timeout = std::chrono::duration(cfg_.lidar_timeout_s); + return have_lidar_ && now - lidar_received_at_ <= timeout; + } + + std::pair lidar_diagnostics(Clock::time_point now) const { + std::lock_guard lock(state_mutex_); + if (!have_lidar_) { + return {-1.0, 0}; + } + return {std::chrono::duration(now - lidar_received_at_).count(), + last_lidar_width_}; + } + geometry_msgs::Twist fresh_command_or_zero(Clock::time_point now) const { std::lock_guard lock(state_mutex_); const auto timeout = std::chrono::duration(cfg_.command_timeout_s); @@ -367,10 +560,32 @@ class M20ROSBridge : public Module { void publish_cycle(bool force_zero) { const auto now = Clock::now(); + const bool cloud_ready = !force_zero && !stopping_.load(std::memory_order_acquire) && + lidar_fresh(now); + std_msgs::Bool lidar_ready_message; + lidar_ready_message.data = static_cast(cloud_ready); + lidar_ready_.publish(lidar_ready_message); + + const int8_t new_lidar_state = cloud_ready ? 1 : 0; + const int8_t previous_lidar_state = + lidar_health_state_.exchange(new_lidar_state, std::memory_order_acq_rel); + if (previous_lidar_state != new_lidar_state) { + const auto [age_s, width] = lidar_diagnostics(now); + if (cloud_ready) { + logging::info("M20 lidar stream is healthy", + {logging::Field("cloud_age_s", age_s), + logging::Field("cloud_width", static_cast(width))}); + } else { + logging::warn("M20 lidar stream is missing or stale", + {logging::Field("cloud_age_s", age_s), + logging::Field("timeout_s", cfg_.lidar_timeout_s)}); + } + } + const bool ready = !force_zero && !stopping_.load(std::memory_order_acquire) && nav_cmd_publisher_ != nullptr && nav_cmd_publisher_->get_subscription_count() > 0 && - safety_ready(now); + cloud_ready && safety_ready(now); std_msgs::Bool ready_message; ready_message.data = static_cast(ready); command_ready_.publish(ready_message); @@ -389,6 +604,7 @@ class M20ROSBridge : public Module { M20ROSBridgeConfig cfg_; Output command_ready_; + Output lidar_ready_; Output lidar_; Output odom_; Output odometry_; @@ -409,12 +625,16 @@ class M20ROSBridge : public Module { Clock::time_point command_received_at_{}; Clock::time_point location_received_at_{}; Clock::time_point hes_received_at_{}; + Clock::time_point lidar_received_at_{}; bool have_command_ = false; bool have_location_ = false; bool have_hes_ = false; + bool have_lidar_ = false; + uint32_t last_lidar_width_ = 0; int location_status_ = 0; int hes_status_ = 1; std::atomic stopping_{false}; + std::atomic lidar_health_state_{-1}; std::atomic command_sequence_{0}; }; diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py index 6e40151915..2aaeea7bdd 100644 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -59,6 +59,7 @@ class M20ROSBridgeConfig(NativeModuleConfig): command_rate_hz: float = Field(default=10.0, gt=0.0) command_timeout_s: float = Field(default=0.4, gt=0.0) safety_timeout_s: float = Field(default=2.5, gt=0.0) + lidar_timeout_s: float = Field(default=0.5, gt=0.0) max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) @@ -75,6 +76,7 @@ class M20ROSBridge(NativeModule, perception.Lidar, perception.Odometry): safe_cmd_vel: In[Twist] command_ready: Out[Bool] + lidar_ready: Out[Bool] lidar: Out[PointCloud2] odom: Out[PoseStamped] odometry: Out[Odometry] diff --git a/dimos/robot/deeprobotics/m20/bridge/test_module.py b/dimos/robot/deeprobotics/m20/bridge/test_module.py new file mode 100644 index 0000000000..8437f536e4 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/bridge/test_module.py @@ -0,0 +1,37 @@ +# 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. + +"""Configuration contract tests for the native M20 ROS bridge.""" + +from pydantic import ValidationError +import pytest + +from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridgeConfig + + +def test_bridge_requires_five_missed_nominal_clouds_before_stale() -> None: + config = M20ROSBridgeConfig() + + assert config.lidar_timeout_s == 0.5 + + +def test_bridge_normalizes_the_vendor_mislabeled_cloud_frame() -> None: + config = M20ROSBridgeConfig() + + assert config.cloud_frame == "base_link" + + +def test_bridge_rejects_nonpositive_lidar_timeout() -> None: + with pytest.raises(ValidationError): + M20ROSBridgeConfig(lidar_timeout_s=0.0) diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index 1e937c2b06..70a927cab4 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -83,6 +83,7 @@ class M20Connection(Module): cmd_vel: In[Twist] command_ready: In[Bool] + lidar_ready: In[Bool] safe_cmd_vel: Out[Twist] armed: Out[Bool] @@ -91,11 +92,13 @@ def __init__(self, **kwargs: Any) -> None: self._lock = RLock() self._armed = False self._command_ready = False + self._lidar_ready = False @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.command_ready.subscribe(self._on_command_ready))) + self.register_disposable(Disposable(self.lidar_ready.subscribe(self._on_lidar_ready))) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) self.safe_cmd_vel.publish(Twist.zero()) self.armed.publish(Bool(False)) @@ -109,8 +112,10 @@ def stop(self) -> None: def arm(self) -> bool: """Allow bounded planner commands to reach the M20 ROS bridge.""" with self._lock: - if self.config.require_command_ready and not self._command_ready: - logger.warning("M20 command gate refused arm: native bridge is not ready") + if self.config.require_command_ready and ( + not self._command_ready or not self._lidar_ready + ): + logger.warning("M20 command gate refused arm: native bridge or lidar is not ready") return False self._armed = True self.armed.publish(Bool(True)) @@ -135,10 +140,16 @@ def is_armed(self) -> bool: @rpc def is_command_ready(self) -> bool: - """Return whether the native bridge reports a fresh safe command path.""" + """Return whether the native bridge reports a fresh, lidar-safe command path.""" with self._lock: return self._command_ready + @rpc + def is_lidar_ready(self) -> bool: + """Return whether the native bridge has received a valid cloud within its timeout.""" + with self._lock: + return self._lidar_ready + @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: """Forward a bounded planar velocity when armed. @@ -169,3 +180,18 @@ def _on_command_ready(self, msg: Bool) -> None: self.safe_cmd_vel.publish(Twist.zero()) self.armed.publish(Bool(False)) logger.warning("M20 command gate disarmed: native bridge lost readiness") + + def _on_lidar_ready(self, msg: Bool) -> None: + ready = bool(msg.data) + with self._lock: + was_armed = self._armed + changed = self._lidar_ready != ready + self._lidar_ready = ready + if not ready: + self._armed = False + if was_armed and not ready: + self.safe_cmd_vel.publish(Twist.zero()) + self.armed.publish(Bool(False)) + logger.warning("M20 command gate disarmed: lidar stream became stale") + elif changed and ready: + logger.info("M20 lidar stream became ready") diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md new file mode 100644 index 0000000000..9e54df3284 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -0,0 +1,118 @@ +# M20 lidar deployment + +DimOS consumes one merged cloud on GOS. The verified data path is: + +```text +front lidar 10.21.33.201 -- MSOP 6691 / DIFOP 7781 --+ + +--> NOS multicast-relay.service +rear lidar 10.21.33.202 -- MSOP 6692 / DIFOP 7782 --+ --> GOS rsdriver.service + --> DDS /LIDAR/POINTS + --> m20_ros_bridge + --> local LCM lidar stream + --> RayTracingVoxelMap +``` + +The two lidars are already extrinsically merged by the vendor driver. On the +inspected robot, `/LIDAR/POINTS` is reliable/volatile at about 9.5 Hz, with +29,000-81,000 points and 0.8-2.1 MB per cloud. Its 26-byte point layout is: + +| field | type | byte offset | +|---|---|---:| +| `x` | float32 | 0 | +| `y` | float32 | 4 | +| `z` | float32 | 8 | +| `intensity` | float32 | 12 | +| `ring` | uint16 | 16 | +| `timestamp` | float64 | 18 | + +## Persistent robot setup + +NOS must have its existing relay enabled. Its vendor unit already uses +`Restart=always` and `RestartSec=2`: + +```bash +sudo systemctl enable --now multicast-relay.service +systemctl is-enabled multicast-relay.service +systemctl is-active multicast-relay.service +``` + +Run those commands on NOS (`10.21.31.106`). + +GOS runs `rsdriver.service` as root for real-time scheduling. Fast DDS therefore +creates root-owned shared-memory files that an unprivileged DimOS process cannot +attach to. From a DimOS checkout on GOS, install the checked-in permission helper +and systemd drop-in, then enable the driver: + +```bash +sudo install -D -o root -g root -m 0755 \ + dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions \ + /usr/local/libexec/dimos-m20-rsdriver-shm-permissions +sudo install -D -o root -g root -m 0644 \ + dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf \ + /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf +sudo systemctl daemon-reload +sudo systemctl enable --now rsdriver.service +``` + +The drop-in is outside the vendor package, so it survives package replacement. +The vendor package's post-install script may disable `rsdriver.service`; re-run +the `enable --now` command after a driver or firmware update. + +## Run DimOS locally on GOS + +Load the local-only LCM URL before launching DimOS. Its explicit 16 MiB receive +buffer is needed for fragmented multi-megabyte clouds: + +```bash +set -a +source dimos/robot/deeprobotics/m20/deploy/dimos-m20.env +set +a +dimos --rerun-open none --rerun-host 0.0.0.0 \ + run deeprobotics-m20-kronknav +``` + +Attach a viewer from another computer without moving the DimOS graph off GOS: + +```bash +dimos-viewer \ + --connect rerun+http://10.21.31.104:9877/proxy \ + --ws-url ws://10.21.31.104:3030/ws +``` + +The default blueprint never creates a `/NAV_CMD` publisher. Use the separate +`deeprobotics-m20-kronknav-control` blueprint only when motion ownership is +intentional; it still starts disarmed. + +## Health and recovery contract + +Before starting DimOS, both checks below must print `enabled` and `active` on +their respective hosts: + +```bash +systemctl is-enabled multicast-relay.service # NOS +systemctl is-active multicast-relay.service + +systemctl is-enabled rsdriver.service # GOS +systemctl is-active rsdriver.service +``` + +As the normal `user` account on GOS, this measures the DDS stream itself rather +than merely checking that the driver process exists: + +```bash +source /opt/robot/scripts/setup_ros2.sh +ros2 topic hz --wall-time --window 100 /LIDAR/POINTS +``` + +The native bridge additionally validates every cloud and publishes +`lidar_ready` at 10 Hz. Five missed nominal frames (0.5 seconds) make it false. +The bridge logs `M20 lidar stream is missing or stale` once on loss and +`M20 lidar stream is healthy` once on recovery. Both the native `/NAV_CMD` +watchdog and `M20Connection` require fresh lidar, so loss immediately forces +zero velocity and disarms the Python gate. DDS rematches automatically after an +`rsdriver.service` restart; DimOS does not need to restart. + +Process supervision cannot make a disconnected or unpowered sensor produce +data. The contract is therefore: restart crashed vendor processes, detect an +invalid or absent stream within 0.5 seconds, fail closed, expose the state, and +recover automatically when valid clouds return. diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions new file mode 100755 index 0000000000..00a163f3a9 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions @@ -0,0 +1,36 @@ +#!/usr/bin/env bash +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +service_pid="${1:-}" +if [[ ! "$service_pid" =~ ^[0-9]+$ ]] || [[ ! -d "/proc/$service_pid" ]]; then + echo "expected the live rsdriver.service MainPID, got: $service_pid" >&2 + exit 1 +fi + +for _attempt in $(seq 1 50); do + driver_pid="$(pgrep --parent "$service_pid" --exact rslidar | head -n 1 || true)" + if [[ -n "$driver_pid" ]]; then + mapfile -t shm_files < <( + { + awk '$NF ~ /^\/dev\/shm\/fastrtps_/ {print $NF}' "/proc/$driver_pid/maps" + for fd in "/proc/$driver_pid"/fd/*; do + readlink -f "$fd" || true + done + } \ + | grep -E '^/dev/shm/fastrtps_([0-9a-f]+|port[0-9]+)(_el)?$' \ + | sort -u + ) + if (( ${#shm_files[@]} >= 4 )); then + chgrp user -- "${shm_files[@]}" + chmod g+rw -- "${shm_files[@]}" + exit 0 + fi + fi + sleep 0.1 +done + +echo "rslidar did not open its Fast DDS shared-memory files within 5 seconds" >&2 +exit 1 diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env b/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env new file mode 100644 index 0000000000..8f55f85fe5 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env @@ -0,0 +1,3 @@ +# Local-only LCM bus with an explicit receive buffer for 0.8-2.1 MB clouds. +DIMOS_TRANSPORT=lcm +LCM_DEFAULT_URL="udpm://239.255.76.67:7667?ttl=0&recv_buf_size=16777216" diff --git a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf new file mode 100644 index 0000000000..ef7b8e627c --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf @@ -0,0 +1,8 @@ +# GOS runs rslidar as root so it can request real-time scheduling. Fast DDS +# creates its SHM segment with mode 0666 masked by this service's UMask. Keep +# root execution, but let the normal onboard `user` account attach to the DDS +# segment used by the DimOS bridge. +[Service] +Group=user +UMask=0002 +ExecStartPost=/usr/local/libexec/dimos-m20-rsdriver-shm-permissions $MAINPID diff --git a/dimos/robot/deeprobotics/m20/test_connection.py b/dimos/robot/deeprobotics/m20/test_connection.py index 0bb560aba0..1fb25beea2 100644 --- a/dimos/robot/deeprobotics/m20/test_connection.py +++ b/dimos/robot/deeprobotics/m20/test_connection.py @@ -105,6 +105,7 @@ def test_connection_forwards_bounded_command_after_arm( armed_publish = mocker.patch.object(connection.armed, "publish") command = Twist(linear=Vector3(0.7, -0.4, 2.0), angular=Vector3(1.0, 2.0, 0.9)) + connection._on_lidar_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() accepted = connection.move(command) @@ -125,6 +126,7 @@ def test_disarm_publishes_zero_and_blocks_following_commands( safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() safe_publish.reset_mock() @@ -144,6 +146,7 @@ def test_connection_refuses_arm_until_native_bridge_is_ready( ) -> None: connection = connection_factory() armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) accepted = connection.arm() @@ -159,6 +162,7 @@ def test_connection_disarms_when_native_bridge_loses_readiness( connection = connection_factory() safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() safe_publish.reset_mock() @@ -171,3 +175,51 @@ def test_connection_disarms_when_native_bridge_loses_readiness( safe_publish.assert_called_once_with(Twist.zero()) armed_publish.assert_called_once() assert armed_publish.call_args.args[0].data is False + + +def test_connection_refuses_arm_without_a_fresh_lidar_stream( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_command_ready(Bool(True)) + + accepted = connection.arm() + + assert accepted is False + assert connection.is_lidar_ready() is False + assert connection.is_armed() is False + armed_publish.assert_not_called() + + +def test_connection_reports_lidar_recovery( + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + + connection._on_lidar_ready(Bool(True)) + + assert connection.is_lidar_ready() is True + + +def test_connection_disarms_when_lidar_stream_becomes_stale( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) + connection._on_command_ready(Bool(True)) + connection.arm() + safe_publish.reset_mock() + armed_publish.reset_mock() + + connection._on_lidar_ready(Bool(False)) + + assert connection.is_lidar_ready() is False + assert connection.is_armed() is False + safe_publish.assert_called_once_with(Twist.zero()) + armed_publish.assert_called_once() + assert armed_publish.call_args.args[0].data is False diff --git a/native/cpp/include/dimos/native/config.hpp b/native/cpp/include/dimos/native/config.hpp index 74a5ad77f2..c1d42068d9 100644 --- a/native/cpp/include/dimos/native/config.hpp +++ b/native/cpp/include/dimos/native/config.hpp @@ -103,6 +103,28 @@ class Config { } } + /// Read and consume one required field without aggregate field-name reflection. + /// + /// Use this on deployment targets whose compiler supports the SDK runtime + /// but not the C++20 non-type template arguments required by + /// `pfr::names_as_array`. Call `enforce_all_consumed()` after the final + /// field to retain the same strict one-to-one config contract as parse(). + template + T take(const std::string& key) { + auto it = obj_.find(key); + if (it == obj_.end()) { + throw std::runtime_error("config: missing required field '" + key + "'"); + } + config_detail::check_json_type(*it, key); + try { + T value = it->template get(); + consumed_.insert(key); + return value; + } catch (const std::exception& e) { + throw std::runtime_error("config: field '" + key + "': " + e.what()); + } + } + /// Deserialize into a plain aggregate struct, enforcing the one-to-one key /// check (every field present, no unknowns) and the optional validate(). template @@ -114,17 +136,7 @@ class Config { constexpr auto names = pfr::names_as_array(); pfr::for_each_field(out, [&](auto& field, std::size_t i) { const std::string key(names[i]); - auto it = obj_.find(key); - if (it == obj_.end()) { - throw std::runtime_error("config: missing required field '" + key + "'"); - } - config_detail::check_json_type>(*it, key); - try { - field = it->template get>(); - } catch (const std::exception& e) { - throw std::runtime_error("config: field '" + key + "': " + e.what()); - } - consumed_.insert(key); + field = take>(key); }); enforce_all_consumed(); config_detail::validate_if_present(out); diff --git a/native/cpp/tests/test_config.cpp b/native/cpp/tests/test_config.cpp index 29bfdf33b7..56965796e3 100644 --- a/native/cpp/tests/test_config.cpp +++ b/native/cpp/tests/test_config.cpp @@ -94,6 +94,27 @@ TEST_CASE("a non-object config is rejected") { CHECK_THROWS_AS(Config(json::array({1, 2})), std::runtime_error); } +TEST_CASE("take reads explicit fields and preserves strict consumption") { + Config cfg(json{{"count", 3}, {"enabled", true}, {"name", "m20"}}); + + CHECK(cfg.take("count") == 3); + CHECK(cfg.take("enabled")); + CHECK(cfg.take("name") == "m20"); + CHECK_NOTHROW(cfg.enforce_all_consumed()); +} + +TEST_CASE("take rejects missing, wrong-typed, and unconsumed fields") { + Config missing(json::object()); + CHECK_THROWS_AS(missing.take("rate"), std::runtime_error); + + Config wrong_type(json{{"enabled", 1}}); + CHECK_THROWS_AS(wrong_type.take("enabled"), std::runtime_error); + + Config unknown(json{{"known", 1}, {"extra", 2}}); + CHECK(unknown.take("known") == 1); + CHECK_THROWS_AS(unknown.enforce_all_consumed(), std::runtime_error); +} + TEST_CASE("parse deserializes a typed config struct") { Config cfg(json{{"value", 5}, {"name", "lidar"}}); RangedCfg c = cfg.parse(); From 323c06a8c2cbcdc3fd120c9231228b3adf31e7ae Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 00:19:53 +0800 Subject: [PATCH 03/15] fix(robot): stabilize native M20 mapping pipeline --- dimos/core/native_module.py | 19 +- dimos/robot/all_blueprints.py | 2 + .../m20/blueprints/m20_kronknav.py | 77 +- .../m20/blueprints/test_m20_kronknav.py | 74 +- dimos/robot/deeprobotics/m20/bridge/README.md | 27 +- .../deeprobotics/m20/bridge/cpp/build.sh | 6 + .../deeprobotics/m20/bridge/cpp/main.cpp | 224 +++-- dimos/robot/deeprobotics/m20/bridge/module.py | 41 +- dimos/robot/deeprobotics/m20/connection.py | 339 ++++++- dimos/robot/deeprobotics/m20/constants.py | 4 +- dimos/robot/deeprobotics/m20/deploy/README.md | 105 ++- .../deploy/dimos-m20-fastdds-permissions.path | 13 + .../dimos-m20-fastdds-permissions.service | 10 + .../dimos-m20-multicast-relay-supervisor | 95 ++ .../deploy/dimos-m20-rsdriver-shm-permissions | 49 +- .../10-dimos-network-readiness.conf | 11 + .../10-dimos-command-ownership.conf | 6 + .../10-dimos-shm-permissions.conf | 7 +- .../deeprobotics/m20/pointlio/__init__.py | 15 + .../m20/pointlio/cpp/CMakeLists.txt | 77 ++ .../deeprobotics/m20/pointlio/cpp/build.sh | 28 + .../m20/pointlio/cpp/compat/glog/logging.h | 8 + .../deeprobotics/m20/pointlio/cpp/main.cpp | 873 ++++++++++++++++++ .../robot/deeprobotics/m20/pointlio/module.py | 146 +++ .../deeprobotics/m20/pointlio/test_module.py | 42 + .../robot/deeprobotics/m20/test_connection.py | 200 +++- 26 files changed, 2320 insertions(+), 178 deletions(-) create mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path create mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service create mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor create mode 100644 dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf create mode 100644 dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf create mode 100644 dimos/robot/deeprobotics/m20/pointlio/__init__.py create mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt create mode 100755 dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh create mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h create mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp create mode 100644 dimos/robot/deeprobotics/m20/pointlio/module.py create mode 100644 dimos/robot/deeprobotics/m20/pointlio/test_module.py diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 84fea6b685..68ede8a293 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -75,8 +75,14 @@ def _set_process_to_die_when_parent_dies() -> None: if _LIBC.prctl(_PR_SET_PDEATHSIG, signal.SIGTERM) != 0: err = ctypes.get_errno() raise OSError(err, f"_set_process_to_die_when_parent_dies failed: {os.strerror(err)}") + + def _configure_native_child(cpu_affinity: frozenset[int] | None) -> None: + _set_process_to_die_when_parent_dies() + if cpu_affinity is not None: + os.sched_setaffinity(0, cpu_affinity) else: _set_process_to_die_when_parent_dies = None # type: ignore[assignment] + _configure_native_child = None # type: ignore[assignment] if sys.version_info < (3, 13): from typing_extensions import TypeVar @@ -122,6 +128,8 @@ class NativeModuleConfig(ModuleConfig): cwd: str | None = None extra_args: list[str] = Field(default_factory=list) extra_env: dict[str, str] = Field(default_factory=dict) + # Optional Linux CPU set inherited by every thread in the native child. + cpu_affinity: frozenset[int] | None = None # Session settings for this module alone, e.g. opening it as the zenoh router # the rest of the graph connects to. None follows the global config. session: SessionConfig | None = None @@ -306,6 +314,15 @@ def start(self) -> None: module=self._module_label, cmd=" ".join(cmd), cwd=cwd, + cpu_affinity=( + sorted(self.config.cpu_affinity) if self.config.cpu_affinity is not None else None + ), + ) + + child_setup = ( + functools.partial(_configure_native_child, self.config.cpu_affinity) + if _configure_native_child is not None + else None ) self._process = subprocess.Popen( @@ -316,7 +333,7 @@ def start(self) -> None: stdout=subprocess.PIPE, stderr=subprocess.PIPE, start_new_session=True, - preexec_fn=_set_process_to_die_when_parent_dies, + preexec_fn=child_setup, ) assert self._process.stdin is not None if stdin_blob is not None: diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index eac1ce3173..a99b6acea1 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -44,6 +44,7 @@ "coordinator-xarm7": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm7", "deeprobotics-m20-kronknav": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav", "deeprobotics-m20-kronknav-control": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav_control", + "deeprobotics-m20-pointlio": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_pointlio", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", @@ -229,6 +230,7 @@ "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", "m20-connection": "dimos.robot.deeprobotics.m20.connection.M20Connection", + "m20-point-lio": "dimos.robot.deeprobotics.m20.pointlio.module.M20PointLio", "m20-ros-bridge": "dimos.robot.deeprobotics.m20.bridge.module.M20ROSBridge", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "manipulation-skills": "dimos.manipulation.manipulation_skills.ManipulationSkills", diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 6c3dd104a3..69a59fff4e 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -28,6 +28,7 @@ from dimos.robot.deeprobotics.m20.connection import M20Connection from dimos.robot.deeprobotics.m20.constants import ( BASE_LINK_HEIGHT_M, + BODY_LENGTH_M, BODY_WIDTH_M, MAX_ANGULAR_Z_RAD_S, MAX_LINEAR_X_M_S, @@ -35,6 +36,7 @@ PLANNING_HEIGHT_M, ROTATION_DIAMETER_M, ) +from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLio from dimos.visualization.vis_module import vis_module VOXEL_SIZE_M = 0.1 @@ -53,29 +55,85 @@ def _render_path(msg: Any) -> Any: return msg +def _static_robot_body(rr: Any) -> list[Any]: + return [ + rr.Boxes3D( + half_sizes=[ + BODY_LENGTH_M * 0.5, + BODY_WIDTH_M * 0.5, + PLANNING_HEIGHT_M * 0.5, + ], + centers=[0.0, 0.0, PLANNING_HEIGHT_M * 0.5 - BASE_LINK_HEIGHT_M], + colors=[(0, 255, 127)], + ), + rr.Transform3D(parent_frame="tf#/base_link"), + ] + + +def _m20_rerun_blueprint() -> Any: + """Go2-style navigation layout, adapted for the camera-less M20.""" + import rerun as rr + import rerun.blueprint as rrb + + return rrb.Blueprint( + rrb.Spatial3DView( + origin="world", + name="M20 KronkNav", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D( + plane=rr.components.Plane3D.XY.with_distance(0.5), + ), + ), + rrb.TimePanel(state="hidden"), + rrb.SelectionPanel(state="hidden"), + ) + + _rerun_config = { - "memory_limit": "256MB", + "blueprint": _m20_rerun_blueprint, + # Match the Go2 navigation replay budget so a newly attached viewer catches + # up quickly instead of replaying a large sensor backlog. + "memory_limit": "64MB", "tf_axes": 0.35, "max_hz": { - "world/lidar": 2.0, - "world/local_map": 2.0, - "world/global_map": 0.2, + "world/local_map": 0.5, + # RayTracingVoxelMap already limits this at the source. + "world/global_map": 0, }, "visual_override": { + # These are internal high-rate bridge streams. Logging the 100k-point + # raw cloud and 200 Hz IMU saturated the RK3588 and queued minutes of + # Rerun data. The Go2 navigation view likewise shows maps, not lidar. + "world/raw_lidar": None, + "world/imu": None, + "world/lidar": None, "world/global_map": _render_global_map, "world/planner_path": None, "world/path": _render_path, **planner_visual_override(PLANNER_VIZ_HZ), }, + "static": { + "world/robot_body": _static_robot_body, + }, } +# Safe hardware bring-up graph: raw M20 sensors -> native Point-LIO -> Rerun. +# It intentionally has no connection/controller modules and cannot publish +# /NAV_CMD. Use this before starting the full mapper/planner blueprint. +deeprobotics_m20_pointlio = autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + M20ROSBridge.blueprint(enable_command_output=False), + M20PointLio.blueprint(), +).global_config(n_workers=2, transport="lcm") + + def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: """Compose one complete M20 graph on GOS. The boolean is intentionally fixed by the two exported blueprints below; selecting the control blueprint is the deployment-time ownership decision. - Both still start with the Python command gate disarmed. + Both still start with the operator command arm disarmed. """ return autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), @@ -85,6 +143,7 @@ def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: max_linear_y=MAX_LINEAR_Y_M_S, max_angular_z=MAX_ANGULAR_Z_RAD_S, ), + M20PointLio.blueprint(), M20Connection.blueprint( max_linear_x=MAX_LINEAR_X_M_S, max_linear_y=MAX_LINEAR_Y_M_S, @@ -94,7 +153,7 @@ def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: voxel_size=VOXEL_SIZE_M, max_range=25.0, emit_every=1, - global_emit_every=20, + global_emit_every=50, support_min=4, world_frame="odom", worker_threads=3, @@ -141,6 +200,8 @@ def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: # does not create a /NAV_CMD publisher and M20Connection can never become ready. deeprobotics_m20_kronknav = autoconnect(_m20_kronknav(enable_command_output=False)) -# Explicit control ownership: creates /NAV_CMD, while still requiring fresh -# estop/localization status and a deliberate M20Connection.arm() RPC. +# Explicit control ownership: creates /NAV_CMD. A single M20Connection.standup() +# call performs the vendor state/gait sequence and arms after the robot confirms +# its RL-Control command path. Mapper health remains navigation diagnostics, as +# it does in the Go2 stack; it is not a manual-teleop latch. deeprobotics_m20_kronknav_control = autoconnect(_m20_kronknav(enable_command_output=True)) diff --git a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py index be89ac90b6..865ce6205b 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py @@ -16,9 +16,11 @@ from dimos.core.coordination.blueprints import Blueprint from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.geometry_msgs.PointStamped import PointStamped from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.tf2_msgs.TFMessage import TFMessage @@ -29,9 +31,14 @@ from dimos.robot.deeprobotics.m20.blueprints.m20_kronknav import ( deeprobotics_m20_kronknav, deeprobotics_m20_kronknav_control, + deeprobotics_m20_pointlio, ) from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge from dimos.robot.deeprobotics.m20.connection import M20Connection +from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLio +from dimos.visualization.rerun.bridge import RerunBridgeModule +from dimos.visualization.rerun.websocket_server import RerunWebSocketServer +from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule def _bridge_kwargs(blueprint: Blueprint) -> dict[str, object]: @@ -40,6 +47,12 @@ def _bridge_kwargs(blueprint: Blueprint) -> dict[str, object]: return atoms[0].kwargs +def _module_kwargs(blueprint: Blueprint, module: type) -> dict[str, object]: + atoms = [atom for atom in blueprint.active_blueprints if atom.module is module] + assert len(atoms) == 1 + return atoms[0].kwargs + + def _endpoint_modules( blueprint: Blueprint, name: str, @@ -63,6 +76,11 @@ def test_default_kronknav_blueprint_cannot_publish_robot_commands() -> None: assert _bridge_kwargs(deeprobotics_m20_kronknav)["enable_command_output"] is False +def test_pointlio_bringup_blueprint_has_no_command_publisher() -> None: + assert _bridge_kwargs(deeprobotics_m20_pointlio)["enable_command_output"] is False + assert not any(atom.module is M20Connection for atom in deeprobotics_m20_pointlio.blueprints) + + def test_control_blueprint_explicitly_enables_robot_command_publisher() -> None: assert _bridge_kwargs(deeprobotics_m20_kronknav_control)["enable_command_output"] is True @@ -75,16 +93,25 @@ def test_m20_blueprints_pin_native_sdk_supported_local_transport() -> None: def test_sensor_and_pose_streams_reach_mapping_and_navigation() -> None: blueprint = deeprobotics_m20_kronknav - assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "in") == {M20PointLio} + assert _endpoint_modules(blueprint, "imu", Imu, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "imu", Imu, "in") == {M20PointLio} + assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20PointLio} assert RayTracingVoxelMap in _endpoint_modules(blueprint, "lidar", PointCloud2, "in") assert _endpoint_modules(blueprint, "lidar_ready", Bool, "out") == {M20ROSBridge} assert _endpoint_modules(blueprint, "lidar_ready", Bool, "in") == {M20Connection} - assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "localization_ready", Bool, "out") == {M20PointLio} + assert _endpoint_modules(blueprint, "localization_ready", Bool, "in") == { + M20ROSBridge, + M20Connection, + } + assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20PointLio} assert _endpoint_modules(blueprint, "tf", TFMessage, "in") == { RayTracingVoxelMap, MLSPlannerNative, } - assert _endpoint_modules(blueprint, "odom", PoseStamped, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "odom", PoseStamped, "out") == {M20PointLio} assert { DanLocalPlanner, DanHolonomicTC, @@ -102,3 +129,44 @@ def test_kronknav_path_and_guarded_command_chain_is_complete() -> None: assert _endpoint_modules(blueprint, "cmd_vel", Twist, "in") == {M20Connection} assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "out") == {M20Connection} assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "in") == {M20ROSBridge} + + +def test_rerun_click_and_teleop_inputs_reach_navigation_and_control() -> None: + blueprint = deeprobotics_m20_kronknav_control + + assert _endpoint_modules(blueprint, "clicked_point", PointStamped, "out") == { + RerunWebSocketServer + } + assert _endpoint_modules(blueprint, "clicked_point", PointStamped, "in") == {MovementManager} + assert _endpoint_modules(blueprint, "tele_cmd_vel", Twist, "out") == { + RerunWebSocketServer, + WebsocketVisModule, + } + assert _endpoint_modules(blueprint, "tele_cmd_vel", Twist, "in") == {MovementManager} + assert _endpoint_modules(blueprint, "goal", PointStamped, "out") == {MovementManager} + assert _endpoint_modules(blueprint, "goal", PointStamped, "in") == { + MLSPlannerNative, + DanLocalPlanner, + } + assert _endpoint_modules(blueprint, "nav_cmd_vel", Twist, "out") == {DanHolonomicTC} + assert _endpoint_modules(blueprint, "nav_cmd_vel", Twist, "in") == {MovementManager} + + +def test_rerun_uses_go2_navigation_data_budget() -> None: + config = _module_kwargs(deeprobotics_m20_kronknav_control, RerunBridgeModule) + visual_override = config["visual_override"] + max_hz = config["max_hz"] + + assert isinstance(visual_override, dict) + assert visual_override["world/raw_lidar"] is None + assert visual_override["world/imu"] is None + assert visual_override["world/lidar"] is None + assert isinstance(max_hz, dict) + assert max_hz["world/local_map"] == 0.5 + assert config["memory_limit"] == "64MB" + + +def test_global_map_is_rate_limited_at_the_source() -> None: + config = _module_kwargs(deeprobotics_m20_kronknav_control, RayTracingVoxelMap) + + assert config["global_emit_every"] == 50 diff --git a/dimos/robot/deeprobotics/m20/bridge/README.md b/dimos/robot/deeprobotics/m20/bridge/README.md index eca3f203f5..83061fcbd9 100644 --- a/dimos/robot/deeprobotics/m20/bridge/README.md +++ b/dimos/robot/deeprobotics/m20/bridge/README.md @@ -8,9 +8,10 @@ The current C++ NativeModule SDK carries those streams over local LCM. The M20 blueprints therefore pin the complete onboard graph to LCM; this bridge does not contain a private Zenoh implementation. -The bridge always subscribes to `/LIDAR/POINTS`, `/ODOM`, `/LOCATION_STATUS`, -and `/HES_STATUS`. It publishes lidar, pose, odometry, TF, lidar-readiness, and -command-readiness streams into the local DimOS graph. +The bridge always subscribes to `/LIDAR/POINTS`, `/IMU`, and `/HES_STATUS`. It +publishes the raw cloud, IMU, lidar-readiness, and command-readiness streams +into the local DimOS graph. `M20PointLio` consumes the cloud and IMU and is the +sole producer of pose, odometry, TF, map-ready lidar, and localization-readiness. The inspected M20 publishes merged front/rear clouds at 10 Hz with reliable, volatile DDS QoS. Each point uses the vendor's 26-byte layout (`x`, `y`, `z`, @@ -32,15 +33,27 @@ become group-writable by the existing `user` group so DimOS remains unprivileged `enable_command_output` defaults to `false`. When explicitly enabled, the bridge owns a `/NAV_CMD` publisher but emits nonzero velocity only while: -- location status is fresh and exactly `1` (normal); -- hard-estop status is fresh and exactly `0` (not triggered); +- the local PointLIO estimate is fresh and advancing; +- `/MOTION_INFO` is fresh and confirms RL Control state `17`; +- any received hard-estop status is exactly `0` (not triggered); - a valid merged lidar cloud has arrived within the lidar timeout; - the `/NAV_CMD` publisher has a matched subscriber; - the Python connection has explicitly armed and supplied a fresh bounded command. +The inspected firmware advertises `/HES_STATUS` with the documented DDS type +and QoS but emits no samples, including to the vendor `ros2 topic echo` tool. +The bridge therefore does not misuse it as a liveness heartbeat: an observed +trigger still vetoes commands, while the robot controller independently +enforces the physical hard stop below this API. + The native watchdog uses a steady clock and sends zero after command timeout, -on lidar or robot-health loss, and during shutdown. Starting the bridge never changes robot -mode, gait, planner service, charging state, or standing state. +on robot-control loss, and during shutdown. Lidar and PointLIO readiness remain +navigation diagnostics; they do not permanently disarm Go2-style manual motion. +Starting the bridge never changes robot mode, gait, planner service, charging +state, or standing state on startup. The normal explicit operator action is one +`M20Connection.standup()` RPC, which switches `basic_server` to navigation usage +mode, completes Stand → RL Control, resets and selects the navigation gait, +waits for command-path feedback, and arms. The bridge diagnoses but does not remotely manage the vendor sensor pipeline. For boot-persistent clouds, `multicast-relay.service` must be enabled on NOS and diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh b/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh index e4a131f2d0..6992060837 100755 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh @@ -30,6 +30,12 @@ cmake_args=( if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") fi +if [[ -n "${M20_PFR_DIR:-}" ]]; then + cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_PFR=${M20_PFR_DIR}") +fi +if [[ -n "${M20_NLOHMANN_JSON_DIR:-}" ]]; then + cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_NLOHMANN_JSON=${M20_NLOHMANN_JSON_DIR}") +fi cmake "${cmake_args[@]}" cmake --build "$bridge_dir/build" --parallel "${M20_BUILD_JOBS:-4}" diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp index 4aa24b08fa..e252745a98 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp @@ -20,25 +20,26 @@ #include #include -#include #include +#include +#include +#include #include -#include #include +#include #include #include #include "dimos/native.hpp" -#include "geometry_msgs/PoseStamped.hpp" -#include "geometry_msgs/TransformStamped.hpp" #include "geometry_msgs/Twist.hpp" -#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/Imu.hpp" #include "sensor_msgs/PointCloud2.hpp" #include "sensor_msgs/PointField.hpp" #include "std_msgs/Bool.hpp" #include "std_msgs/Header.hpp" -#include "tf2_msgs/TFMessage.hpp" +#include "std_msgs/Int32.hpp" +#include "std_msgs/UInt32.hpp" using dimos::native::Builder; using dimos::native::Config; @@ -50,6 +51,7 @@ namespace { using Clock = std::chrono::steady_clock; constexpr int64_t kNanosecondsPerSecond = 1'000'000'000LL; +constexpr int kMotionRlControl = 17; void require_nonempty(const std::string& value, const char* name) { if (value.empty()) { @@ -262,13 +264,14 @@ std_msgs::Header to_dimos_header(const std_msgs::msg::Header& source, struct M20ROSBridgeConfig { std::string lidar_topic; - std::string odom_topic; + std::string imu_topic; std::string nav_cmd_topic; - std::string location_status_topic; + std::string motion_state_topic; + std::string motion_info_topic; + std::string gait_topic; std::string hes_status_topic; std::string node_name; std::string cloud_frame; - std::string world_frame; std::string base_frame; bool enable_command_output; double command_rate_hz; @@ -281,13 +284,14 @@ struct M20ROSBridgeConfig { void validate() const { require_nonempty(lidar_topic, "lidar_topic"); - require_nonempty(odom_topic, "odom_topic"); + require_nonempty(imu_topic, "imu_topic"); require_nonempty(nav_cmd_topic, "nav_cmd_topic"); - require_nonempty(location_status_topic, "location_status_topic"); + require_nonempty(motion_state_topic, "motion_state_topic"); + require_nonempty(motion_info_topic, "motion_info_topic"); + require_nonempty(gait_topic, "gait_topic"); require_nonempty(hes_status_topic, "hes_status_topic"); require_nonempty(node_name, "node_name"); require_nonempty(cloud_frame, "cloud_frame"); - require_nonempty(world_frame, "world_frame"); require_nonempty(base_frame, "base_frame"); dimos::native::require_positive(command_rate_hz, "command_rate_hz"); dimos::native::require_positive(command_timeout_s, "command_timeout_s"); @@ -302,13 +306,14 @@ struct M20ROSBridgeConfig { M20ROSBridgeConfig parse_m20_config(Config& config) { M20ROSBridgeConfig result{}; result.lidar_topic = config.take("lidar_topic"); - result.odom_topic = config.take("odom_topic"); + result.imu_topic = config.take("imu_topic"); result.nav_cmd_topic = config.take("nav_cmd_topic"); - result.location_status_topic = config.take("location_status_topic"); + result.motion_state_topic = config.take("motion_state_topic"); + result.motion_info_topic = config.take("motion_info_topic"); + result.gait_topic = config.take("gait_topic"); result.hes_status_topic = config.take("hes_status_topic"); result.node_name = config.take("node_name"); result.cloud_frame = config.take("cloud_frame"); - result.world_frame = config.take("world_frame"); result.base_frame = config.take("base_frame"); result.enable_command_output = config.take("enable_command_output"); result.command_rate_hz = config.take("command_rate_hz"); @@ -328,12 +333,17 @@ class M20ROSBridge : public Module { void build(Builder& builder, Config& config) override { cfg_ = parse_m20_config(config); builder.input("safe_cmd_vel", &M20ROSBridge::on_command, this); + builder.input("localization_ready", + &M20ROSBridge::on_localization_ready, this); + builder.input("motion_state_cmd", + &M20ROSBridge::on_motion_state_command, this); + builder.input("gait_cmd", &M20ROSBridge::on_gait_command, this); command_ready_ = builder.output("command_ready"); lidar_ready_ = builder.output("lidar_ready"); - lidar_ = builder.output("lidar"); - odom_ = builder.output("odom"); - odometry_ = builder.output("odometry"); - tf_ = builder.output("tf"); + motion_state_ = builder.output("motion_state"); + gait_state_ = builder.output("gait_state"); + raw_lidar_ = builder.output("raw_lidar"); + imu_ = builder.output("imu"); } void setup() override { @@ -349,23 +359,35 @@ class M20ROSBridge : public Module { // future incompatible vendor QoS change instead of silently dropping. const auto lidar_qos = rclcpp::QoS(rclcpp::KeepLast(2)).reliable().durability_volatile(); - const auto sensor_qos = rclcpp::SensorDataQoS().keep_last(2); + // The advertised M20 endpoint is RELIABLE/TRANSIENT_LOCAL. Some M20 + // firmware revisions expose the endpoint without actually emitting + // its documented 1 Hz samples, so HES is a veto when observed rather + // than the command-path heartbeat. The physical stop remains enforced + // below this API by the robot controller. + const auto hes_qos = + rclcpp::QoS(rclcpp::KeepLast(2)).reliable().transient_local(); lidar_subscription_ = node_->create_subscription( cfg_.lidar_topic, lidar_qos, [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { on_lidar(*msg); }); - odom_subscription_ = node_->create_subscription( - cfg_.odom_topic, sensor_qos, - [this](nav_msgs::msg::Odometry::SharedPtr msg) { on_odometry(*msg); }); - location_subscription_ = node_->create_subscription( - cfg_.location_status_topic, sensor_qos, - [this](drdds::msg::LocationStatus::SharedPtr msg) { on_location_status(*msg); }); + imu_subscription_ = node_->create_subscription( + cfg_.imu_topic, + rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(), + [this](sensor_msgs::msg::Imu::SharedPtr msg) { on_imu(*msg); }); hes_subscription_ = node_->create_subscription( - cfg_.hes_status_topic, sensor_qos, + cfg_.hes_status_topic, hes_qos, [this](drdds::msg::StdMsgInt32::SharedPtr msg) { on_hes_status(*msg); }); + motion_info_subscription_ = node_->create_subscription( + cfg_.motion_info_topic, + rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(), + [this](drdds::msg::MotionInfo::SharedPtr msg) { on_motion_info(*msg); }); if (cfg_.enable_command_output) { nav_cmd_publisher_ = node_->create_publisher( cfg_.nav_cmd_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); + motion_state_publisher_ = node_->create_publisher( + cfg_.motion_state_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); + gait_publisher_ = node_->create_publisher( + cfg_.gait_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); } const auto period = std::chrono::duration(1.0 / cfg_.command_rate_hz); @@ -380,7 +402,7 @@ class M20ROSBridge : public Module { logging::info( "M20 ROS bridge started", {logging::Field("lidar_topic", cfg_.lidar_topic), - logging::Field("odom_topic", cfg_.odom_topic), + logging::Field("imu_topic", cfg_.imu_topic), logging::Field("command_output", cfg_.enable_command_output)}); } @@ -399,9 +421,11 @@ class M20ROSBridge : public Module { spin_thread_.join(); } nav_cmd_publisher_.reset(); + motion_state_publisher_.reset(); + gait_publisher_.reset(); hes_subscription_.reset(); - location_subscription_.reset(); - odom_subscription_.reset(); + motion_info_subscription_.reset(); + imu_subscription_.reset(); lidar_subscription_.reset(); if (executor_ != nullptr && node_ != nullptr) { executor_->remove_node(node_); @@ -440,7 +464,7 @@ class M20ROSBridge : public Module { result.data_length = checked_i32(source.data.size(), "point-cloud byte count"); result.data = source.data; result.is_dense = static_cast(source.is_dense); - lidar_.publish(result); + raw_lidar_.publish(result); { std::lock_guard lock(state_mutex_); lidar_received_at_ = Clock::now(); @@ -453,47 +477,27 @@ class M20ROSBridge : public Module { } } - void on_odometry(const nav_msgs::msg::Odometry& source) { - nav_msgs::Odometry result; - result.header = to_dimos_header(source.header, cfg_.world_frame); - result.child_frame_id = cfg_.base_frame; - - result.pose.pose.position.x = source.pose.pose.position.x; - result.pose.pose.position.y = source.pose.pose.position.y; - result.pose.pose.position.z = source.pose.pose.position.z; - result.pose.pose.orientation.x = source.pose.pose.orientation.x; - result.pose.pose.orientation.y = source.pose.pose.orientation.y; - result.pose.pose.orientation.z = source.pose.pose.orientation.z; - result.pose.pose.orientation.w = source.pose.pose.orientation.w; - result.twist.twist.linear.x = source.twist.twist.linear.x; - result.twist.twist.linear.y = source.twist.twist.linear.y; - result.twist.twist.linear.z = source.twist.twist.linear.z; - result.twist.twist.angular.x = source.twist.twist.angular.x; - result.twist.twist.angular.y = source.twist.twist.angular.y; - result.twist.twist.angular.z = source.twist.twist.angular.z; - for (std::size_t i = 0; i < source.pose.covariance.size(); ++i) { - result.pose.covariance[i] = source.pose.covariance[i]; - result.twist.covariance[i] = source.twist.covariance[i]; + void on_imu(const sensor_msgs::msg::Imu& source) { + sensor_msgs::Imu result; + result.header = to_dimos_header(source.header, cfg_.base_frame); + result.orientation.x = source.orientation.x; + result.orientation.y = source.orientation.y; + result.orientation.z = source.orientation.z; + result.orientation.w = source.orientation.w; + result.angular_velocity.x = source.angular_velocity.x; + result.angular_velocity.y = source.angular_velocity.y; + result.angular_velocity.z = source.angular_velocity.z; + result.linear_acceleration.x = source.linear_acceleration.x; + result.linear_acceleration.y = source.linear_acceleration.y; + result.linear_acceleration.z = source.linear_acceleration.z; + for (std::size_t index = 0; index < 9; ++index) { + result.orientation_covariance[index] = source.orientation_covariance[index]; + result.angular_velocity_covariance[index] = + source.angular_velocity_covariance[index]; + result.linear_acceleration_covariance[index] = + source.linear_acceleration_covariance[index]; } - - geometry_msgs::PoseStamped pose; - pose.header = result.header; - pose.pose = result.pose.pose; - - geometry_msgs::TransformStamped transform; - transform.header = result.header; - transform.child_frame_id = cfg_.base_frame; - transform.transform.translation.x = source.pose.pose.position.x; - transform.transform.translation.y = source.pose.pose.position.y; - transform.transform.translation.z = source.pose.pose.position.z; - transform.transform.rotation = result.pose.pose.orientation; - tf2_msgs::TFMessage transforms; - transforms.transforms_length = 1; - transforms.transforms.push_back(std::move(transform)); - - odometry_.publish(result); - odom_.publish(pose); - tf_.publish(transforms); + imu_.publish(result); } void on_command(const geometry_msgs::Twist& source) { @@ -510,28 +514,64 @@ class M20ROSBridge : public Module { have_command_ = true; } - void on_location_status(const drdds::msg::LocationStatus& source) { + void on_localization_ready(const std_msgs::Bool& source) { std::lock_guard lock(state_mutex_); - location_status_ = static_cast(source.data.total_status); - location_received_at_ = Clock::now(); - have_location_ = true; + localization_ready_state_ = source.data != 0; + localization_received_at_ = Clock::now(); + have_localization_ = true; + } + + void on_motion_state_command(const std_msgs::Int32& source) { + if (motion_state_publisher_ == nullptr || !rclcpp::ok()) return; + drdds::msg::MotionState output; + set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); + output.data.state = source.data; + motion_state_publisher_->publish(output); + logging::warn("published M20 motion-state command", + {logging::Field("state", static_cast(source.data))}); + } + + void on_gait_command(const std_msgs::UInt32& source) { + if (gait_publisher_ == nullptr || !rclcpp::ok()) return; + drdds::msg::Gait output; + set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); + output.data.gait = source.data; + gait_publisher_->publish(output); + logging::info("published M20 gait command", + {logging::Field("gait", static_cast(source.data))}); } void on_hes_status(const drdds::msg::StdMsgInt32& source) { std::lock_guard lock(state_mutex_); hes_status_ = vendor_int32_value(source); - hes_received_at_ = Clock::now(); have_hes_ = true; } + void on_motion_info(const drdds::msg::MotionInfo& source) { + const int motion_state = source.data.motion_state.state; + { + std::lock_guard lock(state_mutex_); + motion_state_value_ = motion_state; + motion_info_received_at_ = Clock::now(); + have_motion_info_ = true; + } + std_msgs::Int32 motion; + motion.data = motion_state; + motion_state_.publish(motion); + std_msgs::UInt32 gait; + gait.data = source.data.gait_state.gait; + gait_state_.publish(gait); + } + bool safety_ready(Clock::time_point now) const { std::lock_guard lock(state_mutex_); - if (!cfg_.enable_command_output || !have_location_ || !have_hes_) { + if (!cfg_.enable_command_output || !have_motion_info_) { return false; } const auto timeout = std::chrono::duration(cfg_.safety_timeout_s); - return now - location_received_at_ <= timeout && now - hes_received_at_ <= timeout && - location_status_ == 1 && hes_status_ == 0; + return now - motion_info_received_at_ <= timeout && + motion_state_value_ == kMotionRlControl && + (!have_hes_ || hes_status_ == 0); } bool lidar_fresh(Clock::time_point now) const { @@ -585,7 +625,7 @@ class M20ROSBridge : public Module { const bool ready = !force_zero && !stopping_.load(std::memory_order_acquire) && nav_cmd_publisher_ != nullptr && nav_cmd_publisher_->get_subscription_count() > 0 && - cloud_ready && safety_ready(now); + safety_ready(now); std_msgs::Bool ready_message; ready_message.data = static_cast(ready); command_ready_.publish(ready_message); @@ -605,33 +645,37 @@ class M20ROSBridge : public Module { M20ROSBridgeConfig cfg_; Output command_ready_; Output lidar_ready_; - Output lidar_; - Output odom_; - Output odometry_; - Output tf_; + Output motion_state_; + Output gait_state_; + Output raw_lidar_; + Output imu_; std::shared_ptr node_; std::shared_ptr executor_; rclcpp::Subscription::SharedPtr lidar_subscription_; - rclcpp::Subscription::SharedPtr odom_subscription_; - rclcpp::Subscription::SharedPtr location_subscription_; + rclcpp::Subscription::SharedPtr imu_subscription_; rclcpp::Subscription::SharedPtr hes_subscription_; + rclcpp::Subscription::SharedPtr motion_info_subscription_; rclcpp::Publisher::SharedPtr nav_cmd_publisher_; + rclcpp::Publisher::SharedPtr motion_state_publisher_; + rclcpp::Publisher::SharedPtr gait_publisher_; rclcpp::TimerBase::SharedPtr timer_; std::thread spin_thread_; mutable std::mutex state_mutex_; geometry_msgs::Twist latest_command_ = zero_twist(); Clock::time_point command_received_at_{}; - Clock::time_point location_received_at_{}; - Clock::time_point hes_received_at_{}; + Clock::time_point localization_received_at_{}; + Clock::time_point motion_info_received_at_{}; Clock::time_point lidar_received_at_{}; bool have_command_ = false; - bool have_location_ = false; + bool have_localization_ = false; bool have_hes_ = false; + bool have_motion_info_ = false; bool have_lidar_ = false; + bool localization_ready_state_ = false; uint32_t last_lidar_width_ = 0; - int location_status_ = 0; + int motion_state_value_ = 0; int hes_status_ = 1; std::atomic stopping_{false}; std::atomic lidar_health_state_{-1}; diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py index 2aaeea7bdd..74ec26902f 100644 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -22,18 +22,17 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Bool import Bool -from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.msgs.std_msgs.Int32 import Int32 +from dimos.msgs.std_msgs.UInt32 import UInt32 from dimos.robot.deeprobotics.m20.constants import ( MAX_ANGULAR_Z_RAD_S, MAX_LINEAR_X_M_S, MAX_LINEAR_Y_M_S, ) -from dimos.spec import perception class M20ROSBridgeConfig(NativeModuleConfig): @@ -43,16 +42,26 @@ class M20ROSBridgeConfig(NativeModuleConfig): executable: str = "build/m20_ros_bridge" build_command: str | None = "./build.sh" stdin_config: bool = True + # GOS installs the vendor ROS 2/Foxy and drdds libraries here. NativeModule + # workers do not inherit an interactive shell's ROS setup, so make the + # runtime dependency explicit and reproducible. + extra_env: dict[str, str] = Field( + default_factory=lambda: { + "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", + "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", + } + ) lidar_topic: str = "/LIDAR/POINTS" - odom_topic: str = "/ODOM" + imu_topic: str = "/IMU" nav_cmd_topic: str = "/NAV_CMD" - location_status_topic: str = "/LOCATION_STATUS" + motion_state_topic: str = "/MOTION_STATE" + motion_info_topic: str = "/MOTION_INFO" + gait_topic: str = "/GAIT" hes_status_topic: str = "/HES_STATUS" node_name: str = "dimos_m20_bridge" cloud_frame: str = "base_link" - world_frame: str = "odom" base_frame: str = "base_link" enable_command_output: bool = False @@ -65,22 +74,26 @@ class M20ROSBridgeConfig(NativeModuleConfig): max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) -class M20ROSBridge(NativeModule, perception.Lidar, perception.Odometry): - """Bridge M20 ROS 2 topics to local typed DimOS streams. +class M20ROSBridge(NativeModule): + """Bridge only M20 raw sensors and guarded commands to typed DimOS streams. This process runs on GOS and links against the robot's installed Foxy and - ``drdds`` packages. It does not route through Zenoh or another robot host. + ``drdds`` packages. It has no vendor odometry input: M20PointLio owns the + odometry estimate. It does not route through Zenoh or another robot host. """ config: M20ROSBridgeConfig safe_cmd_vel: In[Twist] + localization_ready: In[Bool] + motion_state_cmd: In[Int32] + gait_cmd: In[UInt32] command_ready: Out[Bool] lidar_ready: Out[Bool] - lidar: Out[PointCloud2] - odom: Out[PoseStamped] - odometry: Out[Odometry] - tf: Out[TFMessage] + motion_state: Out[Int32] + gait_state: Out[UInt32] + raw_lidar: Out[PointCloud2] + imu: Out[Imu] if TYPE_CHECKING: diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index 70a927cab4..7a4fbe6de7 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -16,8 +16,13 @@ from __future__ import annotations +from datetime import datetime +import json import math -from threading import RLock +import socket +import struct +from threading import Condition, RLock +import time from typing import Any from pydantic import Field @@ -29,6 +34,8 @@ from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.std_msgs.Int32 import Int32 +from dimos.msgs.std_msgs.UInt32 import UInt32 from dimos.robot.deeprobotics.m20.constants import ( MAX_ANGULAR_Z_RAD_S, MAX_LINEAR_X_M_S, @@ -38,6 +45,28 @@ logger = setup_logger() +MOTION_IDLE = 0 +MOTION_STAND = 1 +MOTION_SOFT_ESTOP = 2 +MOTION_SIT = 4 +MOTION_RL_CONTROL = 17 + +GAIT_BASIC = 0x1001 +GAIT_STAIR_STANDARD = 0x1003 +GAIT_FLAT_AGILE = 0x3002 +GAIT_STAIR_AGILE = 0x3003 +SUPPORTED_GAITS = {GAIT_BASIC, GAIT_STAIR_STANDARD, GAIT_FLAT_AGILE, GAIT_STAIR_AGILE} + +USAGE_MODE_NORMAL = 0 +USAGE_MODE_NAVIGATION = 1 +USAGE_MODE_ASSISTED = 2 + +_BASIC_SERVER_MAGIC = bytes.fromhex("eb91eb90") +_BASIC_SERVER_JSON = 1 +_BASIC_SERVER_HEADER = struct.Struct("<4sHHB7s") +_BASIC_SERVER_MODE_TYPE = 1101 +_BASIC_SERVER_MODE_COMMAND = 5 + class M20ConnectionConfig(ModuleConfig): """Limits for commands sent to the M20's high-level navigation interface.""" @@ -46,6 +75,13 @@ class M20ConnectionConfig(ModuleConfig): max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) require_command_ready: bool = True + stand_timeout_s: float = Field(default=12.0, gt=0.0) + rl_control_timeout_s: float = Field(default=5.0, gt=0.0) + gait_timeout_s: float = Field(default=5.0, gt=0.0) + control_ready_timeout_s: float = Field(default=15.0, gt=0.0) + basic_server_host: str = "10.21.31.103" + basic_server_tcp_port: int = Field(default=30001, ge=1, le=65535) + basic_server_timeout_s: float = Field(default=3.0, gt=0.0) def _clamp(value: float, limit: float) -> float: @@ -68,15 +104,19 @@ def sanitize_twist(twist: Twist, config: M20ConnectionConfig) -> Twist: class M20Connection(Module): - """Expose the planner-facing M20 command surface with an explicit safety gate. + """Expose the planner-facing M20 command surface with an explicit operator arm. The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, - rejects it while disarmed, bounds planar commands while armed, and emits - ``safe_cmd_vel`` for the robot-local bridge. - - Arming never changes robot motion state, gait, vendor services, or charging - state. Those remain explicit deployment/operator responsibilities. + rejects it until ``standup()`` has armed control, bounds planar commands, and + emits ``safe_cmd_vel`` for the robot-local bridge. Lidar and localization + readiness are diagnostic signals for the navigation stack; like the Go2 + connection, they do not permanently disable manual velocity control. + + ``standup()`` is the normal one-call operator entry point: it completes the + vendor state and gait transitions, waits for the guarded command path, and + arms velocity output. Lower-level RPCs remain available for recovery and + diagnostics. """ config: M20ConnectionConfig @@ -84,22 +124,37 @@ class M20Connection(Module): cmd_vel: In[Twist] command_ready: In[Bool] lidar_ready: In[Bool] + localization_ready: In[Bool] + motion_state: In[Int32] + gait_state: In[UInt32] safe_cmd_vel: Out[Twist] armed: Out[Bool] + motion_state_cmd: Out[Int32] + gait_cmd: Out[UInt32] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._lock = RLock() + self._state_condition = Condition(self._lock) self._armed = False self._command_ready = False self._lidar_ready = False + self._localization_ready = False + self._motion_state: int | None = None + self._gait_state: int | None = None + self._basic_server_message_id = 0 @rpc def start(self) -> None: super().start() self.register_disposable(Disposable(self.command_ready.subscribe(self._on_command_ready))) self.register_disposable(Disposable(self.lidar_ready.subscribe(self._on_lidar_ready))) + self.register_disposable( + Disposable(self.localization_ready.subscribe(self._on_localization_ready)) + ) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) + self.register_disposable(Disposable(self.motion_state.subscribe(self._on_motion_state))) + self.register_disposable(Disposable(self.gait_state.subscribe(self._on_gait_state))) self.safe_cmd_vel.publish(Twist.zero()) self.armed.publish(Bool(False)) @@ -112,10 +167,8 @@ def stop(self) -> None: def arm(self) -> bool: """Allow bounded planner commands to reach the M20 ROS bridge.""" with self._lock: - if self.config.require_command_ready and ( - not self._command_ready or not self._lidar_ready - ): - logger.warning("M20 command gate refused arm: native bridge or lidar is not ready") + if self.config.require_command_ready and not self._command_ready: + logger.warning("M20 command gate refused arm: robot control path is not ready") return False self._armed = True self.armed.publish(Bool(True)) @@ -140,7 +193,7 @@ def is_armed(self) -> bool: @rpc def is_command_ready(self) -> bool: - """Return whether the native bridge reports a fresh, lidar-safe command path.""" + """Return whether the native bridge reports a live robot control path.""" with self._lock: return self._command_ready @@ -150,6 +203,12 @@ def is_lidar_ready(self) -> bool: with self._lock: return self._lidar_ready + @rpc + def is_localization_ready(self) -> bool: + """Return whether the native M20 Point-LIO estimator is healthy and publishing.""" + with self._lock: + return self._localization_ready + @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: """Forward a bounded planar velocity when armed. @@ -169,29 +228,257 @@ def stop_movement(self) -> None: """Publish an immediate zero velocity without changing the arm state.""" self.safe_cmd_vel.publish(Twist.zero()) + @rpc + def standup(self) -> bool: + """Bring the M20 to an armed, navigation-ready standing state.""" + return self.start_control() + + @rpc + def start_control(self) -> bool: + """Prepare and arm the complete M20 velocity-control path in one call.""" + self.disarm() + if not self.enter_navigation_mode(): + logger.error("M20 basic_server rejected the navigation usage mode") + return False + if not self._ensure_rl_control(): + return False + + # Switching usage mode resets the gait to Basic. Confirm that reset, + # then select the documented autonomous-navigation gait. + if not self._set_gait_and_wait(GAIT_BASIC): + logger.error("M20 did not confirm the Basic gait") + return False + if not self._set_gait_and_wait(GAIT_FLAT_AGILE): + logger.error("M20 did not confirm the agile flat navigation gait") + return False + if not self._wait_for_control_readiness(self.config.control_ready_timeout_s): + logger.error("M20 robot control path did not become ready") + return False + return self.arm() + + @rpc + def enter_rl_control(self) -> bool: + """Command the standing M20 to enter RL Control for velocity operation.""" + self.disarm() + self.motion_state_cmd.publish(Int32(MOTION_RL_CONTROL)) + return self._wait_for_motion_state(MOTION_RL_CONTROL, self.config.rl_control_timeout_s) + + @rpc + def enter_navigation_mode(self) -> bool: + """Select the M20 usage mode in which ``/NAV_CMD`` is accepted.""" + return self._set_usage_mode(USAGE_MODE_NAVIGATION) + + @rpc + def liedown(self) -> bool: + """Disarm velocity output and command the M20 to its Sit/prone state.""" + self.disarm() + self.motion_state_cmd.publish(Int32(MOTION_SIT)) + return True + + @rpc + def idle(self) -> bool: + """Disarm velocity output and command the M20 to Idle.""" + self.disarm() + self.motion_state_cmd.publish(Int32(MOTION_IDLE)) + return True + + @rpc + def soft_estop(self) -> bool: + """Disarm velocity output and request the vendor soft emergency stop state.""" + self.disarm() + self.motion_state_cmd.publish(Int32(MOTION_SOFT_ESTOP)) + return True + + @rpc + def set_gait(self, gait: int) -> bool: + """Select a documented M20 gait while stationary in RL Control. + + Supported values are 0x1001 basic, 0x1003 standard stair, 0x3002 + agile flat, and 0x3003 agile stair. + """ + if gait not in SUPPORTED_GAITS: + return False + self.gait_cmd.publish(UInt32(gait)) + return True + + @rpc + def set_navigation_gait(self) -> bool: + """Select the vendor-recommended agile flat gait for autonomous navigation.""" + return self.set_gait(GAIT_FLAT_AGILE) + def _on_command_ready(self, msg: Bool) -> None: ready = bool(msg.data) - with self._lock: + with self._state_condition: was_armed = self._armed self._command_ready = ready - if not ready: - self._armed = False + self._state_condition.notify_all() if was_armed and not ready: self.safe_cmd_vel.publish(Twist.zero()) - self.armed.publish(Bool(False)) - logger.warning("M20 command gate disarmed: native bridge lost readiness") + logger.warning("M20 command output temporarily inhibited: robot control path is stale") def _on_lidar_ready(self, msg: Bool) -> None: ready = bool(msg.data) - with self._lock: - was_armed = self._armed + with self._state_condition: changed = self._lidar_ready != ready self._lidar_ready = ready - if not ready: - self._armed = False - if was_armed and not ready: - self.safe_cmd_vel.publish(Twist.zero()) - self.armed.publish(Bool(False)) - logger.warning("M20 command gate disarmed: lidar stream became stale") - elif changed and ready: + self._state_condition.notify_all() + if changed and ready: logger.info("M20 lidar stream became ready") + + def _on_localization_ready(self, msg: Bool) -> None: + ready = bool(msg.data) + with self._state_condition: + changed = self._localization_ready != ready + self._localization_ready = ready + self._state_condition.notify_all() + if changed and ready: + logger.info("M20 Point-LIO localization became ready") + + def _on_motion_state(self, msg: Int32) -> None: + with self._state_condition: + self._motion_state = int(msg.data) + self._state_condition.notify_all() + + def _on_gait_state(self, msg: UInt32) -> None: + with self._state_condition: + self._gait_state = int(msg.data) + self._state_condition.notify_all() + + def _ensure_rl_control(self) -> bool: + with self._lock: + motion_state = self._motion_state + if motion_state == MOTION_RL_CONTROL: + return True + if motion_state != MOTION_STAND: + self.motion_state_cmd.publish(Int32(MOTION_STAND)) + if not self._wait_for_motion_state(MOTION_STAND, self.config.stand_timeout_s): + logger.error("M20 did not confirm Stand before the transition timeout") + return False + self.motion_state_cmd.publish(Int32(MOTION_RL_CONTROL)) + if not self._wait_for_motion_state(MOTION_RL_CONTROL, self.config.rl_control_timeout_s): + logger.error("M20 did not confirm RL Control after standing") + return False + return True + + def _set_gait_and_wait(self, gait: int) -> bool: + self.gait_cmd.publish(UInt32(gait)) + return self._wait_for_gait_state(gait, self.config.gait_timeout_s) + + def _set_usage_mode(self, mode: int) -> bool: + if mode not in {USAGE_MODE_NORMAL, USAGE_MODE_NAVIGATION, USAGE_MODE_ASSISTED}: + return False + response = self._basic_server_request( + message_type=_BASIC_SERVER_MODE_TYPE, + command=_BASIC_SERVER_MODE_COMMAND, + items={"Mode": mode}, + ) + try: + error_code = int(response["PatrolDevice"]["Items"]["ErrorCode"]) + except (KeyError, TypeError, ValueError): + logger.error("M20 basic_server returned an invalid usage-mode response") + return False + if error_code != 0: + logger.error("M20 basic_server usage-mode switch failed: error %s", error_code) + return False + return True + + def _basic_server_request( + self, + *, + message_type: int, + command: int, + items: dict[str, Any], + ) -> dict[str, Any]: + payload = json.dumps( + { + "PatrolDevice": { + "Type": message_type, + "Command": command, + "Time": datetime.now().strftime("%Y-%m-%d %H:%M:%S"), + "Items": items, + } + }, + separators=(",", ":"), + ).encode() + if len(payload) > 0xFFFF: + raise ValueError("M20 basic_server payload exceeds the APDU limit") + + with self._lock: + message_id = self._basic_server_message_id + self._basic_server_message_id = (message_id + 1) & 0xFFFF + header = _BASIC_SERVER_HEADER.pack( + _BASIC_SERVER_MAGIC, + len(payload), + message_id, + _BASIC_SERVER_JSON, + b"\0" * 7, + ) + + try: + with socket.create_connection( + (self.config.basic_server_host, self.config.basic_server_tcp_port), + timeout=self.config.basic_server_timeout_s, + ) as connection: + connection.settimeout(self.config.basic_server_timeout_s) + connection.sendall(header + payload) + response_header = _recv_exact(connection, _BASIC_SERVER_HEADER.size) + magic, length, response_id, encoding, _reserved = _BASIC_SERVER_HEADER.unpack( + response_header + ) + if magic != _BASIC_SERVER_MAGIC: + raise ValueError("invalid APDU magic") + if response_id != message_id: + raise ValueError("response APDU message ID does not match request") + if encoding != _BASIC_SERVER_JSON: + raise ValueError("basic_server response is not JSON") + response_payload = _recv_exact(connection, length) + except (OSError, ValueError) as exc: + logger.error("M20 basic_server request failed: %s", exc) + return {} + + try: + decoded = json.loads(response_payload) + except (UnicodeDecodeError, json.JSONDecodeError) as exc: + logger.error("M20 basic_server returned invalid JSON: %s", exc) + return {} + return decoded if isinstance(decoded, dict) else {} + + def _wait_for_motion_state(self, expected: int, timeout_s: float) -> bool: + deadline = time.monotonic() + timeout_s + with self._state_condition: + while self._motion_state != expected: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return False + self._state_condition.wait(remaining) + return True + + def _wait_for_gait_state(self, expected: int, timeout_s: float) -> bool: + deadline = time.monotonic() + timeout_s + with self._state_condition: + while self._gait_state != expected: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return False + self._state_condition.wait(remaining) + return True + + def _wait_for_control_readiness(self, timeout_s: float) -> bool: + deadline = time.monotonic() + timeout_s + with self._state_condition: + while not self._command_ready: + remaining = deadline - time.monotonic() + if remaining <= 0.0: + return False + self._state_condition.wait(remaining) + return True + + +def _recv_exact(connection: socket.socket, size: int) -> bytes: + result = bytearray() + while len(result) < size: + chunk = connection.recv(size - len(result)) + if not chunk: + raise OSError("basic_server closed the connection before completing its response") + result.extend(chunk) + return bytes(result) diff --git a/dimos/robot/deeprobotics/m20/constants.py b/dimos/robot/deeprobotics/m20/constants.py index cbfa1feed9..5e4464f3e9 100644 --- a/dimos/robot/deeprobotics/m20/constants.py +++ b/dimos/robot/deeprobotics/m20/constants.py @@ -20,8 +20,8 @@ BODY_LENGTH_M = 0.82 BODY_WIDTH_M = 0.506 -# The official M20 locomotion SDK's stand_height_ default. Verify that the -# vendor /ODOM child pose uses the same base origin before hardware tuning. +# The official M20 locomotion SDK's stand_height_ default. Verify the PointLIO +# base origin and physical clearance on hardware before planner tuning. BASE_LINK_HEIGHT_M = 0.48 # Conservative initial MLS clearance. This includes the body above base_link diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index 9e54df3284..dc69297101 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -8,7 +8,8 @@ front lidar 10.21.33.201 -- MSOP 6691 / DIFOP 7781 --+ rear lidar 10.21.33.202 -- MSOP 6692 / DIFOP 7782 --+ --> GOS rsdriver.service --> DDS /LIDAR/POINTS --> m20_ros_bridge - --> local LCM lidar stream + --> local LCM raw lidar + IMU + --> M20PointLio --> RayTracingVoxelMap ``` @@ -27,10 +28,17 @@ inspected robot, `/LIDAR/POINTS` is reliable/volatile at about 9.5 Hz, with ## Persistent robot setup -NOS must have its existing relay enabled. Its vendor unit already uses -`Restart=always` and `RestartSec=2`: +NOS must have its existing relay enabled. Install the checked-in supervisor and +drop-in before enabling it: ```bash +sudo install -D -o root -g root -m 0755 \ + dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor \ + /usr/local/libexec/dimos-m20-multicast-relay-supervisor +sudo install -D -o root -g root -m 0644 \ + dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf \ + /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf +sudo systemctl daemon-reload sudo systemctl enable --now multicast-relay.service systemctl is-enabled multicast-relay.service systemctl is-active multicast-relay.service @@ -38,6 +46,31 @@ systemctl is-active multicast-relay.service Run those commands on NOS (`10.21.31.106`). +The vendor Python process starts four forwarding threads but does not propagate +a worker-thread failure to systemd. It can therefore remain `active` while one +or both MSOP streams are dead. The supervisor waits for both NOS Ethernet +addresses and multicast routes before launch, then restarts the service if the +process has fewer than its expected four forwarding workers. + +The control blueprint owns `/NAV_CMD`. The M20 manual explicitly requires the +vendor `planner.service` to be stopped before an external publisher uses that +topic. Install the checked-in ownership condition on NOS so a boot script cannot +silently start a second command owner: + +```bash +sudo install -D -o root -g root -m 0644 \ + dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf \ + /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf +sudo systemctl unmask planner.service +sudo systemctl daemon-reload +sudo systemctl stop planner.service +systemctl is-active planner.service # must print inactive +``` + +To deliberately restore the vendor planner, create +`/etc/dimos/enable-vendor-m20-planner` and start the service. Never run it while +the DimOS control blueprint owns `/NAV_CMD`. + GOS runs `rsdriver.service` as root for real-time scheduling. Fast DDS therefore creates root-owned shared-memory files that an unprivileged DimOS process cannot attach to. From a DimOS checkout on GOS, install the checked-in permission helper @@ -58,12 +91,65 @@ The drop-in is outside the vendor package, so it survives package replacement. The vendor package's post-install script may disable `rsdriver.service`; re-run the `enable --now` command after a driver or firmware update. +## Cable-free operator access + +GOS has no Wi-Fi radio. Use the vendor-managed AP on AOS while DimOS continues +to run entirely on GOS. The inspected robot exposes `m20_24G`; NetworkManager +gives clients an address in `10.21.41.0/24`, with AOS at `10.21.41.1`, and +routes them to the internal `10.21.31.0/24` network. + +The AP credential is robot configuration and is intentionally not stored in +this repository. It can be read or changed on AOS using the vendor Wi-Fi tools. +The vendor `start.service` launches `loop_start_ap.sh` at boot, which +recreates the AP when `wlan0` is down. No DimOS service is required on AOS. + +If an old office-client experiment is installed, return AOS to the vendor AP: + +```bash +sudo systemctl disable --now dimos-m20-office-wifi.service 2>/dev/null || true +sudo systemctl disable --now wifi-office-autoswitch.service 2>/dev/null || true +sudo systemctl disable --now zenoh-router.service 2>/dev/null || true +sudo nmcli connection modify office5g connection.autoconnect no 2>/dev/null || true +sudo nmcli connection down office5g 2>/dev/null || true +``` + +After a few seconds, verify on AOS: + +```bash +iw dev wlan0 info +ip -4 address show dev wlan0 +``` + +The output must show `type AP`, the intended SSID, and +`10.21.41.1/24`. Connect the developer computer to that SSID; it should use +DHCP. On macOS, add the robot-subnet route explicitly so a simultaneous USB +phone tether remains the internet default: + +```bash +networksetup -setadditionalroutes \ + "Wi-Fi" 10.21.31.0 255.255.255.0 10.21.41.1 +route -n get 10.21.31.104 +ping 10.21.31.104 +ssh user@10.21.31.104 +dimos-viewer \ + --connect rerun+http://10.21.31.104:9877/proxy \ + --ws-url ws://10.21.31.104:3030/ws +``` + +Use a separate USB phone tether if the developer computer also needs internet; +the robot AP is the robot route, not the office internet connection. No DimOS +module, LCM traffic, ROS/DDS traffic, or Zenoh router runs on AOS. + ## Run DimOS locally on GOS Load the local-only LCM URL before launching DimOS. Its explicit 16 MiB receive -buffer is needed for fragmented multi-megabyte clouds: +buffer is needed for fragmented multi-megabyte clouds. The native ROS bridge +also needs the vendor Foxy library path and Fast DDS profile in its inherited +environment: ```bash +source /opt/ros/foxy/setup.bash +export FASTRTPS_DEFAULT_PROFILES_FILE=/opt/robot/fastdds.xml set -a source dimos/robot/deeprobotics/m20/deploy/dimos-m20.env set +a @@ -71,7 +157,12 @@ dimos --rerun-open none --rerun-host 0.0.0.0 \ run deeprobotics-m20-kronknav ``` -Attach a viewer from another computer without moving the DimOS graph off GOS: +After a daemon launch, use `dimos status` and `dimos log` to confirm that both +`M20ROSBridge` and `M20PointLio` remained alive. A successful viewer connection +alone proves only the visualization process, not the sensor bridge. + +Attach a viewer over direct robot Ethernet or the onboard AP without moving the +DimOS graph off GOS: ```bash dimos-viewer \ @@ -81,7 +172,9 @@ dimos-viewer \ The default blueprint never creates a `/NAV_CMD` publisher. Use the separate `deeprobotics-m20-kronknav-control` blueprint only when motion ownership is -intentional; it still starts disarmed. +intentional; it still starts disarmed. Its single `M20Connection.standup()` RPC +switches `basic_server` to navigation usage mode (`Type=1101`, `Command=5`, +`Mode=1`), transitions to RL Control, selects gait `0x3002`, and arms commands. ## Health and recovery contract diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path new file mode 100644 index 0000000000..a536339e7f --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path @@ -0,0 +1,13 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +[Unit] +Description=Watch M20 Fast DDS shared-memory objects +After=dev-shm.mount + +[Path] +PathChanged=/dev/shm +Unit=dimos-m20-fastdds-permissions.service + +[Install] +WantedBy=multi-user.target diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service new file mode 100644 index 0000000000..3e62199e5f --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service @@ -0,0 +1,10 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +[Unit] +Description=Expose M20 Fast DDS shared memory to the onboard DimOS user +After=dev-shm.mount + +[Service] +Type=oneshot +ExecStart=/usr/local/libexec/dimos-m20-rsdriver-shm-permissions --all diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor new file mode 100644 index 0000000000..5d9d9facbf --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor @@ -0,0 +1,95 @@ +#!/usr/bin/env bash +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +set -uo pipefail + +readonly RECEIVE_INTERFACE="eth0" +readonly RECEIVE_ADDRESS="10.21.33.106" +readonly SEND_INTERFACE="eth1" +readonly SEND_ADDRESS="10.21.31.106" +readonly FRONT_LIDAR_ADDRESS="10.21.33.201" +readonly FRONT_MULTICAST_GROUP="224.10.10.201" +readonly MINIMUM_THREAD_COUNT=5 + +relay_pid="" + +stop_relay() { + if [[ -n "$relay_pid" ]] && kill -0 "$relay_pid" 2>/dev/null; then + kill -TERM "$relay_pid" 2>/dev/null || true + wait "$relay_pid" 2>/dev/null || true + fi +} + +handle_stop() { + stop_relay + exit 0 +} + +network_is_ready() { + [[ "$(cat "/sys/class/net/$RECEIVE_INTERFACE/carrier" 2>/dev/null)" == "1" ]] \ + && [[ "$(cat "/sys/class/net/$SEND_INTERFACE/carrier" 2>/dev/null)" == "1" ]] \ + && ip -4 -o address show dev "$RECEIVE_INTERFACE" \ + | grep -Fq "$RECEIVE_ADDRESS/24" \ + && ip -4 -o address show dev "$SEND_INTERFACE" \ + | grep -Fq "$SEND_ADDRESS/24" \ + && ip -4 route get "$FRONT_LIDAR_ADDRESS" \ + from "$RECEIVE_ADDRESS" oif "$RECEIVE_INTERFACE" >/dev/null 2>&1 \ + && ip -4 route get "$FRONT_MULTICAST_GROUP" \ + from "$SEND_ADDRESS" oif "$SEND_INTERFACE" >/dev/null 2>&1 +} + +thread_count() { + find "/proc/$relay_pid/task" -mindepth 1 -maxdepth 1 -type d \ + 2>/dev/null | wc -l +} + +trap handle_stop TERM INT + +for _attempt in $(seq 1 180); do + if network_is_ready; then + break + fi + sleep 0.5 +done + +if ! network_is_ready; then + echo "M20 multicast relay network did not become ready within 90 seconds" >&2 + exit 1 +fi + +/usr/bin/python3 /usr/bin/multicast.py & +relay_pid=$! + +for _attempt in $(seq 1 50); do + if ! kill -0 "$relay_pid" 2>/dev/null; then + wait "$relay_pid" 2>/dev/null + exit $? + fi + if [[ "$(thread_count)" -ge "$MINIMUM_THREAD_COUNT" ]]; then + break + fi + sleep 0.1 +done + +if [[ "$(thread_count)" -lt "$MINIMUM_THREAD_COUNT" ]]; then + echo "M20 multicast relay did not start all four forwarding workers" >&2 + stop_relay + exit 1 +fi + +while kill -0 "$relay_pid" 2>/dev/null; do + if ! network_is_ready; then + echo "M20 multicast relay lost its required network path; restarting" >&2 + stop_relay + exit 1 + fi + if [[ "$(thread_count)" -lt "$MINIMUM_THREAD_COUNT" ]]; then + echo "M20 multicast relay lost a forwarding worker; restarting" >&2 + stop_relay + exit 1 + fi + sleep 1 +done + +wait "$relay_pid" diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions index 00a163f3a9..9b68ddb252 100755 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions @@ -4,16 +4,48 @@ set -euo pipefail +apply_fastdds_permissions() { + local path + local -a shm_files=() + + mapfile -d '' -t shm_files < <( + find /dev/shm -maxdepth 1 -type f \ + \( -name 'fastrtps_*' -o -name 'sem.fastrtps_*' \) -print0 + ) + + for path in "${shm_files[@]}"; do + [[ -e "$path" ]] || continue + chgrp user -- "$path" || { + [[ ! -e "$path" ]] || return 1 + continue + } + chmod g+rw -- "$path" || { + [[ ! -e "$path" ]] || return 1 + } + done +} + +if [[ "${1:-}" == "--all" ]]; then + # Fast DDS creates a segment, port, lock and semaphore in separate steps. + # Rescan briefly so a single systemd path activation covers the whole set. + for _attempt in $(seq 1 20); do + apply_fastdds_permissions + sleep 0.1 + done + exit 0 +fi + service_pid="${1:-}" if [[ ! "$service_pid" =~ ^[0-9]+$ ]] || [[ ! -d "/proc/$service_pid" ]]; then echo "expected the live rsdriver.service MainPID, got: $service_pid" >&2 exit 1 fi +settle_passes=0 for _attempt in $(seq 1 50); do driver_pid="$(pgrep --parent "$service_pid" --exact rslidar | head -n 1 || true)" if [[ -n "$driver_pid" ]]; then - mapfile -t shm_files < <( + mapfile -t driver_shm_files < <( { awk '$NF ~ /^\/dev\/shm\/fastrtps_/ {print $NF}' "/proc/$driver_pid/maps" for fd in "/proc/$driver_pid"/fd/*; do @@ -23,12 +55,19 @@ for _attempt in $(seq 1 50); do | grep -E '^/dev/shm/fastrtps_([0-9a-f]+|port[0-9]+)(_el)?$' \ | sort -u ) - if (( ${#shm_files[@]} >= 4 )); then - chgrp user -- "${shm_files[@]}" - chmod g+rw -- "${shm_files[@]}" - exit 0 + if (( ${#driver_shm_files[@]} >= 4 )); then + ((settle_passes += 1)) fi fi + + # A Foxy reader scans every same-host Fast DDS participant before matching + # rslidar. Root-owned encryption/cpu segments can therefore block the cloud + # even when rslidar's own files are writable. + apply_fastdds_permissions + + if (( settle_passes >= 20 )); then + exit 0 + fi sleep 0.1 done diff --git a/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf b/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf new file mode 100644 index 0000000000..6fc1e61abf --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf @@ -0,0 +1,11 @@ +[Unit] +Wants=network-online.target +After=network-online.target +StartLimitIntervalSec=0 + +[Service] +ExecStart= +ExecStart=/usr/local/libexec/dimos-m20-multicast-relay-supervisor +Restart=always +RestartSec=2 +TimeoutStopSec=5 diff --git a/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf b/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf new file mode 100644 index 0000000000..9afb99a887 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf @@ -0,0 +1,6 @@ +# DimOS owns /NAV_CMD on M20 integrations. The vendor manual requires its +# planner service to be stopped before an external publisher uses that topic. +# Create /etc/dimos/enable-vendor-m20-planner to deliberately restore the +# vendor planner instead of allowing two command owners by accident. +[Unit] +ConditionPathExists=/etc/dimos/enable-vendor-m20-planner diff --git a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf index ef7b8e627c..a82d6144ac 100644 --- a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf +++ b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf @@ -1,7 +1,6 @@ -# GOS runs rslidar as root so it can request real-time scheduling. Fast DDS -# creates its SHM segment with mode 0666 masked by this service's UMask. Keep -# root execution, but let the normal onboard `user` account attach to the DDS -# segment used by the DimOS bridge. +# GOS runs rslidar as root so it can request real-time scheduling and publish a +# usable Fast DDS writer on this vendor image. Keep root execution, but let the +# normal onboard `user` account attach to the local DDS shared-memory objects. [Service] Group=user UMask=0002 diff --git a/dimos/robot/deeprobotics/m20/pointlio/__init__.py b/dimos/robot/deeprobotics/m20/pointlio/__init__.py new file mode 100644 index 0000000000..1dc502c8bb --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/__init__.py @@ -0,0 +1,15 @@ +# 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. + +"""DimOS-native Point-LIO adapter for the Deep Robotics M20.""" diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt new file mode 100644 index 0000000000..d001ebeb0a --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.14) +project(m20_pointlio CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(Eigen3 REQUIRED) +find_package(OpenMP QUIET) +find_package(PCL 1.8 REQUIRED COMPONENTS common filters) +find_package(PkgConfig REQUIRED) +pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) + +include(FetchContent) + +if(DEFINED DIMOS_LCM_DIR) + set(dimos_lcm_SOURCE_DIR ${DIMOS_LCM_DIR}) +else() + FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG 0a1c24710ce2f7a569e1673617102cc5254a75e2 + ) + FetchContent_GetProperties(dimos_lcm) + if(NOT dimos_lcm_POPULATED) + FetchContent_Populate(dimos_lcm) + endif() +endif() + +if(NOT DEFINED POINTLIO_DIR) + FetchContent_Declare(pointlio + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-module-pointlio.git + GIT_TAG 82ef3a327347e2866e981bd95c8bece8b72903cf + ) + FetchContent_GetProperties(pointlio) + if(NOT pointlio_POPULATED) + FetchContent_Populate(pointlio) + endif() + set(POINTLIO_DIR ${pointlio_SOURCE_DIR}) +endif() + +if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) + set(DIMOS_NATIVE_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../native/cpp) +endif() +add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) + +add_executable(m20_pointlio + main.cpp + ${POINTLIO_DIR}/src/preprocess.cpp + ${POINTLIO_DIR}/src/Estimator.cpp + ${POINTLIO_DIR}/src/parameters.cpp +) +target_include_directories(m20_pointlio PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/compat + ${POINTLIO_DIR}/include + ${POINTLIO_DIR}/include/IKFoM/IKFoM_toolkit + ${POINTLIO_DIR}/src + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs + ${PCL_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../hardware/sensors/lidar/common +) +target_compile_definitions(m20_pointlio PRIVATE + MP_PROC_NUM=1 + ROOT_DIR="/tmp/m20_pointlio_" +) +target_compile_options(m20_pointlio PRIVATE -Wall -Wextra -Wpedantic) +target_link_libraries(m20_pointlio PRIVATE + dimos_native + Eigen3::Eigen + PkgConfig::LCM + ${PCL_LIBRARIES} +) +if(OpenMP_CXX_FOUND) + target_link_libraries(m20_pointlio PRIVATE OpenMP::OpenMP_CXX) +endif() diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh b/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh new file mode 100755 index 0000000000..679d9c4d0a --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +pointlio_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +cmake_args=( + -S "$pointlio_dir" + -B "$pointlio_dir/build" + -DCMAKE_BUILD_TYPE=Release +) +if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then + cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") +fi +if [[ -n "${M20_POINTLIO_DIR:-}" ]]; then + cmake_args+=("-DPOINTLIO_DIR=${M20_POINTLIO_DIR}") +fi +if [[ -n "${M20_PFR_DIR:-}" ]]; then + cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_PFR=${M20_PFR_DIR}") +fi +if [[ -n "${M20_NLOHMANN_JSON_DIR:-}" ]]; then + cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_NLOHMANN_JSON=${M20_NLOHMANN_JSON_DIR}") +fi + +cmake "${cmake_args[@]}" +cmake --build "$pointlio_dir/build" --parallel "${M20_BUILD_JOBS:-4}" diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h b/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h new file mode 100644 index 0000000000..e4dd8dc1af --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h @@ -0,0 +1,8 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// The pinned Point-LIO iVox header includes glog, but all LOG calls in that +// header are commented out. Keep the M20 build free of an unused system glog +// dependency while preserving the upstream source unchanged. + +#pragma once diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp new file mode 100644 index 0000000000..f7a3bc6c07 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp @@ -0,0 +1,873 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// M20 Point-LIO adapter. The hardware bridge publishes the robot's public +// merged RoboSense PointCloud2 and base-aligned IMU onto local DimOS LCM +// streams. This process converts those typed streams into the existing DimOS +// Point-LIO core and owns odom -> base_link. It has no vendor odometry input. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "dimos/native.hpp" + +#include "geometry_msgs/PoseStamped.hpp" +#include "geometry_msgs/TransformStamped.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/Imu.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Bool.hpp" +#include "tf2_msgs/TFMessage.hpp" + +#include "estimator_pose.hpp" +#include "point_cloud_utils.hpp" + +// Existing DimOS Point-LIO core, pinned by CMake. +#include "pointlio.hpp" +#include "pointlio_debug.hpp" + +using dimos::native::Builder; +using dimos::native::Config; +using dimos::native::Module; +using dimos::native::Output; +namespace logging = dimos::native::log; + +namespace { + +using Clock = std::chrono::steady_clock; +constexpr double kStandardGravityMps2 = 9.80665; +constexpr std::size_t kPointLioStaticPointLimit = 100'000; +constexpr std::size_t kM20RawPointLimit = 500'000; +constexpr std::size_t kMaxInitializationLidarFrames = 20; + +void require_nonempty(const std::string& value, const char* name) { + if (value.empty()) { + throw std::runtime_error(std::string(name) + " must not be empty"); + } +} + +void require_vector_size(const std::vector& value, std::size_t expected, + const char* name) { + if (value.size() != expected) { + throw std::runtime_error(std::string(name) + " must contain " + + std::to_string(expected) + " values"); + } +} + +int ivox_nearby_code(const std::string& name) { + if (name == "center") return 0; + if (name == "nearby6") return 6; + if (name == "nearby18") return 18; + if (name == "nearby26") return 26; + throw std::runtime_error( + "ivox_nearby_type must be one of: center nearby6 nearby18 nearby26, got '" + + name + "'"); +} + +double header_seconds(const std_msgs::Header& header) { + return static_cast(header.stamp.sec) + + static_cast(header.stamp.nsec) / 1e9; +} + +template +T read_unaligned(const std::vector& data, std::size_t offset) { + T result{}; + std::memcpy(&result, data.data() + offset, sizeof(T)); + return result; +} + +struct M20FieldOffsets { + std::size_t x; + std::size_t y; + std::size_t z; + std::size_t intensity; + std::size_t ring; + std::size_t timestamp; +}; + +M20FieldOffsets validate_m20_cloud(const sensor_msgs::PointCloud2& cloud) { + if (cloud.height <= 0 || cloud.width <= 0) { + throw std::runtime_error("M20 point cloud is empty"); + } + if (cloud.is_bigendian != 0) { + throw std::runtime_error("M20 Point-LIO requires a little-endian cloud"); + } + if (cloud.point_step <= 0 || cloud.row_step <= 0) { + throw std::runtime_error("M20 point cloud has an invalid stride"); + } + const auto point_count = static_cast(cloud.width) * + static_cast(cloud.height); + if (point_count > kM20RawPointLimit) { + throw std::runtime_error("M20 point cloud exceeds the raw input sanity limit"); + } + const auto required_bytes = point_count * static_cast(cloud.point_step); + if (required_bytes > cloud.data.size()) { + throw std::runtime_error("M20 point cloud data is shorter than its dimensions"); + } + + constexpr auto missing = std::numeric_limits::max(); + M20FieldOffsets offsets{missing, missing, missing, missing, missing, missing}; + for (const auto& field : cloud.fields) { + const auto offset = static_cast(field.offset); + if (field.count <= 0 || offset >= static_cast(cloud.point_step)) { + continue; + } + if (field.name == "x" && field.datatype == sensor_msgs::PointField::FLOAT32) { + offsets.x = offset; + } else if (field.name == "y" && + field.datatype == sensor_msgs::PointField::FLOAT32) { + offsets.y = offset; + } else if (field.name == "z" && + field.datatype == sensor_msgs::PointField::FLOAT32) { + offsets.z = offset; + } else if (field.name == "intensity" && + field.datatype == sensor_msgs::PointField::FLOAT32) { + offsets.intensity = offset; + } else if (field.name == "ring" && + field.datatype == sensor_msgs::PointField::UINT16) { + offsets.ring = offset; + } else if (field.name == "timestamp" && + field.datatype == sensor_msgs::PointField::FLOAT64) { + offsets.timestamp = offset; + } + } + if (offsets.x == missing || offsets.y == missing || offsets.z == missing || + offsets.intensity == missing || offsets.ring == missing || + offsets.timestamp == missing) { + throw std::runtime_error( + "M20 Point-LIO requires float32 x/y/z/intensity, uint16 ring, and " + "float64 timestamp fields"); + } + if (offsets.timestamp + sizeof(double) > static_cast(cloud.point_step) || + offsets.ring + sizeof(uint16_t) > static_cast(cloud.point_step) || + offsets.intensity + sizeof(float) > static_cast(cloud.point_step)) { + throw std::runtime_error("M20 point fields extend past point_step"); + } + return offsets; +} + +struct TimedPoint { + double timestamp; + custom_messages::CustomPoint point; +}; + +} // namespace + +struct M20PointLioConfig { + std::string world_frame; + std::string base_frame; + double processing_rate_hz; + double pointcloud_rate_hz; + double odometry_rate_hz; + double readiness_rate_hz; + double lidar_timeout_s; + double imu_timeout_s; + double estimate_timeout_s; + double max_scan_duration_s; + int max_cloud_points; + double msr_freq; + double main_freq; + bool con_frame; + int con_frame_num; + bool cut_frame; + double cut_frame_time_interval; + double time_lag_imu_to_lidar; + int scan_line; + int scan_rate; + double blind; + int point_filter_num; + bool use_imu_as_input; + bool prop_at_freq_of_imu; + bool check_satu; + int init_map_size; + bool space_down_sample; + double satu_acc; + double satu_gyro; + double acc_norm; + double plane_thr; + double filter_size_surf; + double filter_size_map; + double ivox_grid_resolution; + std::string ivox_nearby_type; + double cube_side_length; + double det_range; + double fov_degree; + bool imu_en; + bool start_in_aggressive_motion; + bool extrinsic_est_en; + double imu_time_inte; + double lidar_meas_cov; + double acc_cov_input; + double vel_cov; + double gyr_cov_input; + double gyr_cov_output; + double acc_cov_output; + double b_gyr_cov; + double b_acc_cov; + double imu_meas_acc_cov; + double imu_meas_omg_cov; + double match_s; + bool gravity_align; + std::vector gravity; + std::vector gravity_init; + std::vector extrinsic_t; + std::vector extrinsic_r; + bool publish_odometry_without_downsample; + bool odom_only; + bool debug; + + void validate() const { + require_nonempty(world_frame, "world_frame"); + require_nonempty(base_frame, "base_frame"); + dimos::native::require_positive(processing_rate_hz, "processing_rate_hz"); + dimos::native::require_positive(pointcloud_rate_hz, "pointcloud_rate_hz"); + dimos::native::require_positive(odometry_rate_hz, "odometry_rate_hz"); + dimos::native::require_positive(readiness_rate_hz, "readiness_rate_hz"); + dimos::native::require_positive(lidar_timeout_s, "lidar_timeout_s"); + dimos::native::require_positive(imu_timeout_s, "imu_timeout_s"); + dimos::native::require_positive(estimate_timeout_s, "estimate_timeout_s"); + dimos::native::require_positive(max_scan_duration_s, "max_scan_duration_s"); + dimos::native::require_positive(msr_freq, "msr_freq"); + dimos::native::require_positive(main_freq, "main_freq"); + if (max_cloud_points <= 0 || + max_cloud_points > static_cast(kPointLioStaticPointLimit)) { + throw std::runtime_error("max_cloud_points must be in [1, 100000]"); + } + if (scan_line <= 0 || scan_line > std::numeric_limits::max()) { + throw std::runtime_error("scan_line must be in [1, 65535]"); + } + if (point_filter_num <= 0 || init_map_size <= 0 || con_frame_num <= 0) { + throw std::runtime_error( + "point_filter_num, init_map_size, and con_frame_num must be positive"); + } + require_vector_size(gravity, 3, "gravity"); + require_vector_size(gravity_init, 3, "gravity_init"); + require_vector_size(extrinsic_t, 3, "extrinsic_t"); + require_vector_size(extrinsic_r, 9, "extrinsic_r"); + (void)ivox_nearby_code(ivox_nearby_type); + } +}; + +// GOS ships GCC 9. Its C++20 implementation is sufficient for the native SDK +// and Point-LIO, but not for Boost.PFR's compile-time field-name extraction. +// Parse keys explicitly so the source builds with the robot's stock toolchain +// while retaining strict unknown/missing-key validation. +M20PointLioConfig parse_m20_pointlio_config(Config& config) { + M20PointLioConfig result{}; + result.world_frame = config.take("world_frame"); + result.base_frame = config.take("base_frame"); + result.processing_rate_hz = config.take("processing_rate_hz"); + result.pointcloud_rate_hz = config.take("pointcloud_rate_hz"); + result.odometry_rate_hz = config.take("odometry_rate_hz"); + result.readiness_rate_hz = config.take("readiness_rate_hz"); + result.lidar_timeout_s = config.take("lidar_timeout_s"); + result.imu_timeout_s = config.take("imu_timeout_s"); + result.estimate_timeout_s = config.take("estimate_timeout_s"); + result.max_scan_duration_s = config.take("max_scan_duration_s"); + result.max_cloud_points = config.take("max_cloud_points"); + result.msr_freq = config.take("msr_freq"); + result.main_freq = config.take("main_freq"); + result.con_frame = config.take("con_frame"); + result.con_frame_num = config.take("con_frame_num"); + result.cut_frame = config.take("cut_frame"); + result.cut_frame_time_interval = config.take("cut_frame_time_interval"); + result.time_lag_imu_to_lidar = config.take("time_lag_imu_to_lidar"); + result.scan_line = config.take("scan_line"); + result.scan_rate = config.take("scan_rate"); + result.blind = config.take("blind"); + result.point_filter_num = config.take("point_filter_num"); + result.use_imu_as_input = config.take("use_imu_as_input"); + result.prop_at_freq_of_imu = config.take("prop_at_freq_of_imu"); + result.check_satu = config.take("check_satu"); + result.init_map_size = config.take("init_map_size"); + result.space_down_sample = config.take("space_down_sample"); + result.satu_acc = config.take("satu_acc"); + result.satu_gyro = config.take("satu_gyro"); + result.acc_norm = config.take("acc_norm"); + result.plane_thr = config.take("plane_thr"); + result.filter_size_surf = config.take("filter_size_surf"); + result.filter_size_map = config.take("filter_size_map"); + result.ivox_grid_resolution = config.take("ivox_grid_resolution"); + result.ivox_nearby_type = config.take("ivox_nearby_type"); + result.cube_side_length = config.take("cube_side_length"); + result.det_range = config.take("det_range"); + result.fov_degree = config.take("fov_degree"); + result.imu_en = config.take("imu_en"); + result.start_in_aggressive_motion = config.take("start_in_aggressive_motion"); + result.extrinsic_est_en = config.take("extrinsic_est_en"); + result.imu_time_inte = config.take("imu_time_inte"); + result.lidar_meas_cov = config.take("lidar_meas_cov"); + result.acc_cov_input = config.take("acc_cov_input"); + result.vel_cov = config.take("vel_cov"); + result.gyr_cov_input = config.take("gyr_cov_input"); + result.gyr_cov_output = config.take("gyr_cov_output"); + result.acc_cov_output = config.take("acc_cov_output"); + result.b_gyr_cov = config.take("b_gyr_cov"); + result.b_acc_cov = config.take("b_acc_cov"); + result.imu_meas_acc_cov = config.take("imu_meas_acc_cov"); + result.imu_meas_omg_cov = config.take("imu_meas_omg_cov"); + result.match_s = config.take("match_s"); + result.gravity_align = config.take("gravity_align"); + result.gravity = config.take>("gravity"); + result.gravity_init = config.take>("gravity_init"); + result.extrinsic_t = config.take>("extrinsic_t"); + result.extrinsic_r = config.take>("extrinsic_r"); + result.publish_odometry_without_downsample = + config.take("publish_odometry_without_downsample"); + result.odom_only = config.take("odom_only"); + result.debug = config.take("debug"); + config.enforce_all_consumed(); + result.validate(); + return result; +} + +class M20PointLio : public Module { +public: + void build(Builder& builder, Config& config) override { + cfg_ = parse_m20_pointlio_config(config); + + builder.input("raw_lidar", &M20PointLio::on_lidar, this); + builder.input("imu", &M20PointLio::on_imu, this); + localization_ready_ = builder.output("localization_ready"); + lidar_ = builder.output("lidar"); + odom_ = builder.output("odom"); + odometry_ = builder.output("odometry"); + tf_ = builder.output("tf"); + + process_period_ = std::chrono::duration_cast( + std::chrono::duration(1.0 / cfg_.processing_rate_hz)); + pointcloud_period_ = std::chrono::duration_cast( + std::chrono::duration(1.0 / cfg_.pointcloud_rate_hz)); + odometry_period_ = std::chrono::duration_cast( + std::chrono::duration(1.0 / cfg_.odometry_rate_hz)); + readiness_period_ = std::chrono::duration_cast( + std::chrono::duration(1.0 / cfg_.readiness_rate_hz)); + } + + void setup() override { + pointlio_debug = cfg_.debug; + + PointLioParams params; + params.odom_header_frame_id = cfg_.world_frame; + params.odom_child_frame_id = cfg_.base_frame; + params.con_frame = cfg_.con_frame; + params.con_frame_num = cfg_.con_frame_num; + params.cut_frame = cfg_.cut_frame; + params.cut_frame_time_interval = cfg_.cut_frame_time_interval; + params.time_lag_imu_to_lidar = cfg_.time_lag_imu_to_lidar; + // The M20 converter emits the same timestamped CustomMsg shape as the + // existing Mid-360 adapter, so the core intentionally stays in AVIA mode. + params.lidar_type = 1; + params.scan_line = cfg_.scan_line; + params.scan_rate = cfg_.scan_rate; + params.timestamp_unit = 3; + params.blind = cfg_.blind; + params.point_filter_num = cfg_.point_filter_num; + params.use_imu_as_input = cfg_.use_imu_as_input; + params.prop_at_freq_of_imu = cfg_.prop_at_freq_of_imu; + params.check_satu = cfg_.check_satu; + params.init_map_size = cfg_.init_map_size; + params.space_down_sample = cfg_.space_down_sample; + params.satu_acc = cfg_.satu_acc; + params.satu_gyro = cfg_.satu_gyro; + params.acc_norm = cfg_.acc_norm; + params.plane_thr = static_cast(cfg_.plane_thr); + params.filter_size_surf = cfg_.filter_size_surf; + params.filter_size_map = cfg_.filter_size_map; + params.ivox_grid_resolution = static_cast(cfg_.ivox_grid_resolution); + params.ivox_nearby_type = ivox_nearby_code(cfg_.ivox_nearby_type); + params.cube_side_length = cfg_.cube_side_length; + params.det_range = static_cast(cfg_.det_range); + params.fov_degree = cfg_.fov_degree; + params.imu_en = cfg_.imu_en; + params.start_in_aggressive_motion = cfg_.start_in_aggressive_motion; + params.extrinsic_est_en = cfg_.extrinsic_est_en; + params.imu_time_inte = cfg_.imu_time_inte; + params.lidar_meas_cov = cfg_.lidar_meas_cov; + params.acc_cov_input = cfg_.acc_cov_input; + params.vel_cov = cfg_.vel_cov; + params.gyr_cov_input = cfg_.gyr_cov_input; + params.gyr_cov_output = cfg_.gyr_cov_output; + params.acc_cov_output = cfg_.acc_cov_output; + params.b_gyr_cov = cfg_.b_gyr_cov; + params.b_acc_cov = cfg_.b_acc_cov; + params.imu_meas_acc_cov = cfg_.imu_meas_acc_cov; + params.imu_meas_omg_cov = cfg_.imu_meas_omg_cov; + params.match_s = cfg_.match_s; + params.gravity_align = cfg_.gravity_align; + params.gravity = cfg_.gravity; + params.gravity_init = cfg_.gravity_init; + params.extrinsic_T = cfg_.extrinsic_t; + params.extrinsic_R = cfg_.extrinsic_r; + params.publish_odometry_without_downsample = + cfg_.publish_odometry_without_downsample; + params.odom_only = cfg_.odom_only; + + point_lio_ = std::make_unique(params, cfg_.msr_freq, cfg_.main_freq); + const auto now = Clock::now(); + last_pointcloud_publish_ = now; + last_odometry_publish_ = now; + last_readiness_publish_ = now - readiness_period_; + processing_thread_ = std::thread([this]() { processing_loop(); }); + logging::info("M20 Point-LIO started", + {logging::Field("world_frame", cfg_.world_frame), + logging::Field("base_frame", cfg_.base_frame), + logging::Field("scan_lines", static_cast(cfg_.scan_line))}); + } + + void teardown() override { + stopping_.store(true, std::memory_order_release); + if (processing_thread_.joinable()) { + processing_thread_.join(); + } + std_msgs::Bool ready; + ready.data = 0; + localization_ready_.publish(ready); + point_lio_.reset(); + } + +private: + void on_lidar(const sensor_msgs::PointCloud2& source) { + if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; + const auto received_at = Clock::now(); + bool feed_reserved = false; + + std::unique_lock callback_lock(lidar_callback_mutex_, std::try_to_lock); + if (!callback_lock.owns_lock() || point_lio_is_processing_lidar()) { + log_busy_lidar_drop(); + return; + } + + try { + const auto offsets = validate_m20_cloud(source); + const auto point_count = static_cast(source.width) * + static_cast(source.height); + const auto point_step = static_cast(source.point_step); + const auto point_limit = static_cast(cfg_.max_cloud_points); + const auto source_sample_count = std::min(point_count, point_limit); + std::vector points; + points.reserve(source_sample_count); + uint16_t min_ring = std::numeric_limits::max(); + uint16_t max_ring = 0; + + for (std::size_t sample_index = 0; sample_index < source_sample_count; + ++sample_index) { + const std::size_t index = + source_sample_count == point_count + ? sample_index + : (source_sample_count == 1 + ? point_count / 2 + : sample_index * (point_count - 1) / + (source_sample_count - 1)); + const auto base = index * point_step; + const float x = read_unaligned(source.data, base + offsets.x); + const float y = read_unaligned(source.data, base + offsets.y); + const float z = read_unaligned(source.data, base + offsets.z); + const float intensity = + read_unaligned(source.data, base + offsets.intensity); + const uint16_t ring = + read_unaligned(source.data, base + offsets.ring); + const double timestamp = + read_unaligned(source.data, base + offsets.timestamp); + if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || + !std::isfinite(timestamp) || ring >= cfg_.scan_line) { + continue; + } + + custom_messages::CustomPoint point{}; + point.x = x; + point.y = y; + point.z = z; + point.reflectivity = static_cast(std::clamp( + std::isfinite(intensity) ? static_cast(intensity) : 0.0, 0.0, + 255.0)); + point.tag = 0; + point.line = ring; + points.push_back({timestamp, point}); + min_ring = std::min(min_ring, ring); + max_ring = std::max(max_ring, ring); + } + if (points.empty()) { + throw std::runtime_error("M20 cloud has no finite Point-LIO returns"); + } + + if (point_count > source_sample_count && + !logged_cloud_sampling_.exchange(true, std::memory_order_acq_rel)) { + logging::info( + "uniformly sampled M20 cloud before Point-LIO preprocessing", + {logging::Field("input_points", static_cast(point_count)), + logging::Field("sampled_source_points", + static_cast(source_sample_count)), + logging::Field("selected_valid_points", + static_cast(points.size()))}); + } + + std::sort(points.begin(), points.end(), + [](const TimedPoint& left, const TimedPoint& right) { + return left.timestamp < right.timestamp; + }); + const auto valid_point_count = points.size(); + const double first_point_time = points.front().timestamp; + const double last_point_time = points.back().timestamp; + const double scan_duration = last_point_time - first_point_time; + if (scan_duration < 0.0 || scan_duration > cfg_.max_scan_duration_s) { + throw std::runtime_error("M20 per-point timestamp span is outside the " + "configured scan-duration limit"); + } + + const double source_header_time = header_seconds(source.header); + // rsdriver uses absolute PTP seconds today. Accept a relative + // per-scan timestamp too, but anchor that explicitly to the header. + const bool absolute_point_time = first_point_time > 100'000'000.0; + const double frame_time = absolute_point_time + ? first_point_time + : source_header_time + first_point_time; + if (!std::isfinite(frame_time) || frame_time <= 0.0) { + throw std::runtime_error("M20 cloud has no usable sensor timestamp"); + } + if (last_lidar_sensor_time_ > 0.0 && frame_time <= last_lidar_sensor_time_) { + logging::warn("dropping non-monotonic M20 lidar frame", + {logging::Field("stamp", frame_time), + logging::Field("previous_stamp", last_lidar_sensor_time_)}); + return; + } + + auto message = boost::make_shared(); + message->header.seq = 0; + message->header.stamp = custom_messages::Time().fromSec(frame_time); + message->header.frame_id = cfg_.base_frame; + message->timebase = static_cast(std::llround(frame_time * 1e9)); + message->lidar_id = 0; + for (auto& reserved : message->rsvd) reserved = 0; + message->points.reserve(points.size()); + for (auto& timed : points) { + const double offset_ns = (timed.timestamp - first_point_time) * 1e9; + timed.point.offset_time = + static_cast(std::max(0.0, std::round(offset_ns))); + message->points.push_back(timed.point); + } + message->point_num = static_cast(message->points.size()); + + // Sensor liveness is an ingress property. Record it as soon as the + // sample is validated, independently of estimator throughput. + { + std::lock_guard lock(health_mutex_); + last_lidar_received_at_ = received_at; + have_lidar_ = true; + } + // Point-LIO's feeder callbacks take its internal buffer mutex and + // are designed to run concurrently with process(). An outer lock + // here would block sensor ingestion for the full estimator step. + { + std::lock_guard lock(lidar_feed_mutex_); + if (estimator_initialized_) { + lidar_feed_pending_ = true; + } else { + ++initialization_lidar_frames_; + } + feed_reserved = true; + } + point_lio_->feed_lidar(message); + last_lidar_sensor_time_ = frame_time; + + if (!logged_cloud_contract_.exchange(true, std::memory_order_acq_rel)) { + logging::info( + "M20 Point-LIO accepted cloud contract", + {logging::Field("input_points", static_cast(point_count)), + logging::Field("valid_points", + static_cast(valid_point_count)), + logging::Field("selected_points", + static_cast(points.size())), + logging::Field("min_ring", static_cast(min_ring)), + logging::Field("max_ring", static_cast(max_ring)), + logging::Field("scan_duration_s", scan_duration), + logging::Field("absolute_point_time", absolute_point_time)}); + } + } catch (const std::exception& error) { + if (feed_reserved) { + std::lock_guard lock(lidar_feed_mutex_); + if (estimator_initialized_) { + lidar_feed_pending_ = false; + } else if (initialization_lidar_frames_ > 0) { + --initialization_lidar_frames_; + } + } + logging::error("dropping M20 cloud before Point-LIO", + {logging::Field("error", std::string(error.what()))}); + } + } + + bool point_lio_is_processing_lidar() const { + std::lock_guard lock(lidar_feed_mutex_); + if (!estimator_initialized_) { + return initialization_lidar_frames_ >= kMaxInitializationLidarFrames; + } + return lidar_feed_pending_; + } + + void log_busy_lidar_drop() { + const auto dropped = + busy_lidar_drops_.fetch_add(1, std::memory_order_acq_rel) + 1; + if (dropped == 1 || dropped % 500 == 0) { + logging::info( + "shedding M20 lidar frame while Point-LIO processes the previous frame", + {logging::Field("dropped", static_cast(dropped))}); + } + } + + void on_imu(const sensor_msgs::Imu& source) { + if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; + const auto received_at = Clock::now(); + const double timestamp = header_seconds(source.header); + if (!std::isfinite(timestamp) || timestamp <= 0.0 || + !std::isfinite(source.angular_velocity.x) || + !std::isfinite(source.angular_velocity.y) || + !std::isfinite(source.angular_velocity.z) || + !std::isfinite(source.linear_acceleration.x) || + !std::isfinite(source.linear_acceleration.y) || + !std::isfinite(source.linear_acceleration.z)) { + logging::error("dropping invalid M20 IMU sample"); + return; + } + if (last_imu_sensor_time_ > 0.0 && timestamp <= last_imu_sensor_time_) { + logging::warn("dropping non-monotonic M20 IMU sample", + {logging::Field("stamp", timestamp), + logging::Field("previous_stamp", last_imu_sensor_time_)}); + return; + } + + auto message = boost::make_shared(); + message->header.seq = 0; + message->header.stamp = custom_messages::Time().fromSec(timestamp); + message->header.frame_id = cfg_.base_frame; + message->orientation.x = source.orientation.x; + message->orientation.y = source.orientation.y; + message->orientation.z = source.orientation.z; + message->orientation.w = source.orientation.w; + message->angular_velocity.x = source.angular_velocity.x; + message->angular_velocity.y = source.angular_velocity.y; + message->angular_velocity.z = source.angular_velocity.z; + // ROS sensor_msgs/Imu is m/s^2; this Point-LIO core expects g. + message->linear_acceleration.x = source.linear_acceleration.x / kStandardGravityMps2; + message->linear_acceleration.y = source.linear_acceleration.y / kStandardGravityMps2; + message->linear_acceleration.z = source.linear_acceleration.z / kStandardGravityMps2; + for (int index = 0; index < 9; ++index) { + message->orientation_covariance[index] = source.orientation_covariance[index]; + message->angular_velocity_covariance[index] = + source.angular_velocity_covariance[index]; + message->linear_acceleration_covariance[index] = + source.linear_acceleration_covariance[index] / + (kStandardGravityMps2 * kStandardGravityMps2); + } + + { + std::lock_guard lock(health_mutex_); + last_imu_received_at_ = received_at; + have_imu_ = true; + } + point_lio_->feed_imu(message); + last_imu_sensor_time_ = timestamp; + } + + bool localization_health_is_fresh(Clock::time_point now) const { + std::lock_guard lock(health_mutex_); + return have_lidar_ && have_imu_ && have_estimate_ && + now - last_lidar_received_at_ <= + std::chrono::duration(cfg_.lidar_timeout_s) && + now - last_imu_received_at_ <= + std::chrono::duration(cfg_.imu_timeout_s) && + now - last_estimate_advanced_at_ <= + std::chrono::duration(cfg_.estimate_timeout_s); + } + + void processing_loop() { + while (!stopping_.load(std::memory_order_acquire)) { + const auto iteration_started = Clock::now(); + bool have_estimate = false; + double estimate_stamp = 0.0; + point_lio_->process(); + const auto pose = point_lio_->get_pose(); + have_estimate = dimos::has_estimate(pose); + if (have_estimate) { + const auto& source_odom = point_lio_->get_odometry(); + estimate_stamp = source_odom.header.stamp.toSec(); + const auto now = Clock::now(); + if (std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { + if (now - last_pointcloud_publish_ >= pointcloud_period_ && + estimate_stamp > last_pointcloud_stamp_) { + const auto cloud = point_lio_->get_body_cloud(); + if (cloud != nullptr && !cloud->empty()) { + publish_pointcloud(cloud, estimate_stamp); + last_pointcloud_stamp_ = estimate_stamp; + last_pointcloud_publish_ = now; + } + } + if (now - last_odometry_publish_ >= odometry_period_ && + estimate_stamp > last_odometry_stamp_) { + publish_odometry(source_odom, estimate_stamp); + last_odometry_stamp_ = estimate_stamp; + last_odometry_publish_ = now; + } + } + } + + const auto now = Clock::now(); + bool estimate_advanced = false; + if (have_estimate && std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { + std::lock_guard lock(health_mutex_); + if (!have_estimate_ || estimate_stamp > last_estimate_sensor_time_) { + last_estimate_sensor_time_ = estimate_stamp; + last_estimate_advanced_at_ = now; + have_estimate_ = true; + estimate_advanced = true; + } + } + if (estimate_advanced) { + std::lock_guard lock(lidar_feed_mutex_); + estimator_initialized_ = true; + lidar_feed_pending_ = false; + } + if (now - last_readiness_publish_ >= readiness_period_) { + publish_readiness(now); + last_readiness_publish_ = now; + } + + const auto elapsed = Clock::now() - iteration_started; + if (elapsed < process_period_) { + std::this_thread::sleep_for(process_period_ - elapsed); + } + } + } + + void publish_readiness(Clock::time_point now) { + const bool ready = localization_health_is_fresh(now); + std_msgs::Bool message; + message.data = static_cast(ready); + localization_ready_.publish(message); + + const int8_t current = ready ? 1 : 0; + const int8_t previous = + readiness_state_.exchange(current, std::memory_order_acq_rel); + if (current != previous) { + if (ready) { + logging::info("M20 Point-LIO localization is ready"); + } else { + logging::warn("M20 Point-LIO localization is not ready"); + } + } + } + + void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double timestamp) { + const auto count = static_cast(cloud->size()); + auto output = dimos::make_xyzi_cloud(cfg_.base_frame, timestamp, count); + for (int index = 0; index < count; ++index) { + float* point = dimos::xyzi_point(output, index); + point[0] = cloud->points[index].x; + point[1] = cloud->points[index].y; + point[2] = cloud->points[index].z; + point[3] = cloud->points[index].intensity; + } + lidar_.publish(output); + } + + void publish_odometry(const custom_messages::Odometry& source, double timestamp) { + nav_msgs::Odometry output; + output.header = dimos::make_header(cfg_.world_frame, timestamp); + output.child_frame_id = cfg_.base_frame; + output.pose.pose.position.x = source.pose.pose.position.x; + output.pose.pose.position.y = source.pose.pose.position.y; + output.pose.pose.position.z = source.pose.pose.position.z; + output.pose.pose.orientation.x = source.pose.pose.orientation.x; + output.pose.pose.orientation.y = source.pose.pose.orientation.y; + output.pose.pose.orientation.z = source.pose.pose.orientation.z; + output.pose.pose.orientation.w = source.pose.pose.orientation.w; + output.twist.twist.linear.x = source.twist.twist.linear.x; + output.twist.twist.linear.y = source.twist.twist.linear.y; + output.twist.twist.linear.z = source.twist.twist.linear.z; + output.twist.twist.angular.x = source.twist.twist.angular.x; + output.twist.twist.angular.y = source.twist.twist.angular.y; + output.twist.twist.angular.z = source.twist.twist.angular.z; + for (int index = 0; index < 36; ++index) { + output.pose.covariance[index] = source.pose.covariance[index]; + output.twist.covariance[index] = source.twist.covariance[index]; + } + + geometry_msgs::PoseStamped pose; + pose.header = output.header; + pose.pose = output.pose.pose; + + geometry_msgs::TransformStamped transform; + transform.header = output.header; + transform.child_frame_id = cfg_.base_frame; + transform.transform.translation.x = output.pose.pose.position.x; + transform.transform.translation.y = output.pose.pose.position.y; + transform.transform.translation.z = output.pose.pose.position.z; + transform.transform.rotation = output.pose.pose.orientation; + tf2_msgs::TFMessage transforms; + transforms.transforms_length = 1; + transforms.transforms.push_back(std::move(transform)); + + odometry_.publish(output); + odom_.publish(pose); + tf_.publish(transforms); + } + + M20PointLioConfig cfg_; + Output localization_ready_; + Output lidar_; + Output odom_; + Output odometry_; + Output tf_; + std::unique_ptr point_lio_; + std::thread processing_thread_; + + Clock::duration process_period_{}; + Clock::duration pointcloud_period_{}; + Clock::duration odometry_period_{}; + Clock::duration readiness_period_{}; + Clock::time_point last_pointcloud_publish_{}; + Clock::time_point last_odometry_publish_{}; + Clock::time_point last_readiness_publish_{}; + double last_pointcloud_stamp_ = 0.0; + double last_odometry_stamp_ = 0.0; + double last_lidar_sensor_time_ = 0.0; + double last_imu_sensor_time_ = 0.0; + + mutable std::mutex health_mutex_; + Clock::time_point last_lidar_received_at_{}; + Clock::time_point last_imu_received_at_{}; + Clock::time_point last_estimate_advanced_at_{}; + double last_estimate_sensor_time_ = 0.0; + bool have_lidar_ = false; + bool have_imu_ = false; + bool have_estimate_ = false; + std::atomic stopping_{false}; + mutable std::mutex lidar_feed_mutex_; + std::mutex lidar_callback_mutex_; + std::size_t initialization_lidar_frames_ = 0; + bool estimator_initialized_ = false; + bool lidar_feed_pending_ = false; + std::atomic busy_lidar_drops_{0}; + std::atomic logged_cloud_contract_{false}; + std::atomic logged_cloud_sampling_{false}; + std::atomic readiness_state_{-1}; +}; + +int main() { + dimos::native::run_with_transport(); + return 0; +} diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py new file mode 100644 index 0000000000..4e7fb09da1 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/module.py @@ -0,0 +1,146 @@ +# 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. + +"""Native Point-LIO wrapper for the M20's typed raw LiDAR and IMU streams.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Literal + +from pydantic import Field + +from dimos.core.native_module import NativeModule, NativeModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.sensor_msgs.Imu import Imu +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.spec import perception + +IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] + + +class M20PointLioConfig(NativeModuleConfig): + """M20 Point-LIO input contract and robot-specific estimator tuning.""" + + cwd: str | None = "cpp" + executable: str = "build/m20_pointlio" + build_command: str | None = "./build.sh" + stdin_config: bool = True + # GOS isolates its RK3588 big cores. Cores 6-7 run the vendor lidar + # drivers, so Point-LIO owns the otherwise-idle big cores 4-5. + cpu_affinity: frozenset[int] | None = frozenset({4, 5}) + + world_frame: str = "odom" + base_frame: str = "base_link" + processing_rate_hz: float = Field(default=1000.0, gt=0.0) + pointcloud_rate_hz: float = Field(default=10.0, gt=0.0) + odometry_rate_hz: float = Field(default=50.0, gt=0.0) + readiness_rate_hz: float = Field(default=10.0, gt=0.0) + lidar_timeout_s: float = Field(default=0.5, gt=0.0) + imu_timeout_s: float = Field(default=0.5, gt=0.0) + estimate_timeout_s: float = Field(default=0.5, gt=0.0) + max_scan_duration_s: float = Field(default=0.2, gt=0.0) + # Live merged M20 frames contain roughly 100k returns. Point-LIO cannot + # process that rate in real time on the RK3588, so the native adapter + # uniformly selects this many returns before sorting and preprocessing. + max_cloud_points: int = Field(default=20_000, gt=0, le=100_000) + + msr_freq: float = Field(default=200.0, gt=0.0) + main_freq: float = Field(default=1000.0, gt=0.0) + con_frame: bool = False + con_frame_num: int = Field(default=1, gt=0) + cut_frame: bool = False + cut_frame_time_interval: float = Field(default=0.1, gt=0.0) + time_lag_imu_to_lidar: float = 0.0 + # The vendor merged cloud offsets the second lidar's rings: front uses + # 0-95 and rear uses 96-191. Treating this as a single 96-line lidar drops + # the complete rear scan in the native adapter before Point-LIO sees it. + scan_line: int = Field(default=192, gt=0) + scan_rate: int = Field(default=10, gt=0) + blind: float = Field(default=0.5, ge=0.0) + point_filter_num: int = Field(default=3, gt=0) + + use_imu_as_input: bool = False + prop_at_freq_of_imu: bool = True + check_satu: bool = True + init_map_size: int = Field(default=10, gt=0) + space_down_sample: bool = True + satu_acc: float = Field(default=3.0, gt=0.0) + satu_gyro: float = Field(default=35.0, gt=0.0) + # Point-LIO expects acceleration in g. The native adapter converts the + # M20's ROS-standard m/s^2 values before feeding the estimator. + acc_norm: float = Field(default=1.0, gt=0.0) + plane_thr: float = Field(default=0.1, gt=0.0) + filter_size_surf: float = Field(default=0.2, gt=0.0) + filter_size_map: float = Field(default=0.5, gt=0.0) + ivox_grid_resolution: float = Field(default=2.0, gt=0.0) + ivox_nearby_type: IvoxNearbyType = "nearby6" + cube_side_length: float = Field(default=1000.0, gt=0.0) + det_range: float = Field(default=60.0, gt=0.0) + fov_degree: float = Field(default=360.0, gt=0.0, le=360.0) + imu_en: bool = True + start_in_aggressive_motion: bool = False + extrinsic_est_en: bool = False + imu_time_inte: float = Field(default=0.005, gt=0.0) + lidar_meas_cov: float = Field(default=0.01, gt=0.0) + acc_cov_input: float = Field(default=0.1, gt=0.0) + vel_cov: float = Field(default=20.0, gt=0.0) + gyr_cov_input: float = Field(default=0.01, gt=0.0) + gyr_cov_output: float = Field(default=1000.0, gt=0.0) + acc_cov_output: float = Field(default=500.0, gt=0.0) + b_gyr_cov: float = Field(default=0.0001, gt=0.0) + b_acc_cov: float = Field(default=0.0001, gt=0.0) + imu_meas_acc_cov: float = Field(default=0.01, gt=0.0) + imu_meas_omg_cov: float = Field(default=0.01, gt=0.0) + match_s: float = Field(default=81.0, gt=0.0) + gravity_align: bool = True + gravity: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + gravity_init: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) + # Both public M20 streams are already expressed in base_link. Identity is + # therefore intentional: Point-LIO owns odom -> base_link with no hidden + # sensor/body transform. + extrinsic_t: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0]) + extrinsic_r: list[float] = Field( + default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + ) + publish_odometry_without_downsample: bool = False + odom_only: bool = False + debug: bool = False + + +class M20PointLio(NativeModule, perception.Lidar, perception.Odometry): + """Run the existing DimOS Point-LIO core on M20 raw sensor streams. + + The module deliberately has no vendor odometry input. It consumes only the + merged ``base_link`` cloud and 200 Hz ``base_link`` IMU produced by the M20 + hardware bridge, then owns the ``odom -> base_link`` transform. + """ + + config: M20PointLioConfig + + raw_lidar: In[PointCloud2] + imu: In[Imu] + + localization_ready: Out[Bool] + lidar: Out[PointCloud2] + odom: Out[PoseStamped] + odometry: Out[Odometry] + tf: Out[TFMessage] + + +if TYPE_CHECKING: + M20PointLio() diff --git a/dimos/robot/deeprobotics/m20/pointlio/test_module.py b/dimos/robot/deeprobotics/m20/pointlio/test_module.py new file mode 100644 index 0000000000..c0fc068610 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/test_module.py @@ -0,0 +1,42 @@ +# 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. + +"""Configuration contract tests for the native M20 Point-LIO adapter.""" + +from pydantic import ValidationError +import pytest + +from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLioConfig + + +def test_pointlio_matches_verified_m20_sensor_contract() -> None: + config = M20PointLioConfig() + + assert config.base_frame == "base_link" + assert config.world_frame == "odom" + assert config.scan_line == 192 + assert config.scan_rate == 10 + assert config.msr_freq == 200.0 + assert config.imu_time_inte == 0.005 + assert config.lidar_timeout_s == 0.5 + assert config.imu_timeout_s == 0.5 + assert config.estimate_timeout_s == 0.5 + assert config.max_cloud_points == 20_000 + assert config.extrinsic_t == [0.0, 0.0, 0.0] + assert config.extrinsic_r == [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] + + +def test_pointlio_rejects_cloud_limit_above_native_static_capacity() -> None: + with pytest.raises(ValidationError): + M20PointLioConfig(max_cloud_points=100_001) diff --git a/dimos/robot/deeprobotics/m20/test_connection.py b/dimos/robot/deeprobotics/m20/test_connection.py index 1fb25beea2..a2db2e1241 100644 --- a/dimos/robot/deeprobotics/m20/test_connection.py +++ b/dimos/robot/deeprobotics/m20/test_connection.py @@ -15,7 +15,9 @@ """Behavior tests for the guarded M20 command surface.""" from collections.abc import Callable, Iterator +import json import math +import struct import pytest from pytest_mock import MockerFixture @@ -25,6 +27,8 @@ from dimos.msgs.std_msgs.Bool import Bool from dimos.protocol.rpc.pubsubrpc import LCMRPC from dimos.robot.deeprobotics.m20.connection import ( + GAIT_BASIC, + GAIT_FLAT_AGILE, M20Connection, M20ConnectionConfig, sanitize_twist, @@ -106,6 +110,7 @@ def test_connection_forwards_bounded_command_after_arm( command = Twist(linear=Vector3(0.7, -0.4, 2.0), angular=Vector3(1.0, 2.0, 0.9)) connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() accepted = connection.move(command) @@ -127,6 +132,7 @@ def test_disarm_publishes_zero_and_blocks_following_commands( armed_publish = mocker.patch.object(connection.armed, "publish") connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() safe_publish.reset_mock() @@ -147,6 +153,7 @@ def test_connection_refuses_arm_until_native_bridge_is_ready( connection = connection_factory() armed_publish = mocker.patch.object(connection.armed, "publish") connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) accepted = connection.arm() @@ -155,7 +162,7 @@ def test_connection_refuses_arm_until_native_bridge_is_ready( armed_publish.assert_not_called() -def test_connection_disarms_when_native_bridge_loses_readiness( +def test_connection_temporarily_inhibits_output_without_clearing_operator_arm( mocker: MockerFixture, connection_factory: Callable[..., M20Connection], ) -> None: @@ -163,6 +170,7 @@ def test_connection_disarms_when_native_bridge_loses_readiness( safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") armed_publish = mocker.patch.object(connection.armed, "publish") connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() safe_publish.reset_mock() @@ -171,26 +179,156 @@ def test_connection_disarms_when_native_bridge_loses_readiness( connection._on_command_ready(Bool(False)) assert connection.is_command_ready() is False - assert connection.is_armed() is False + assert connection.is_armed() is True safe_publish.assert_called_once_with(Twist.zero()) - armed_publish.assert_called_once() - assert armed_publish.call_args.args[0].data is False + armed_publish.assert_not_called() + + connection._on_command_ready(Bool(True)) + assert connection.move(Twist(linear=Vector3(0.2, 0.0, 0.0))) is True + assert safe_publish.call_args.args[0].linear.x == 0.2 + + +def test_standup_runs_complete_control_start_sequence( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + enter_navigation_mode = mocker.patch.object( + connection, "enter_navigation_mode", return_value=True + ) + ensure_rl_control = mocker.patch.object(connection, "_ensure_rl_control", return_value=True) + set_gait = mocker.patch.object(connection, "_set_gait_and_wait", return_value=True) + wait_ready = mocker.patch.object(connection, "_wait_for_control_readiness", return_value=True) + arm = mocker.patch.object(connection, "arm", return_value=True) + + assert connection.standup() is True + + enter_navigation_mode.assert_called_once_with() + ensure_rl_control.assert_called_once_with() + assert set_gait.call_args_list == [mocker.call(GAIT_BASIC), mocker.call(GAIT_FLAT_AGILE)] + wait_ready.assert_called_once_with(15.0) + arm.assert_called_once_with() + + +def test_standup_stops_when_navigation_usage_mode_is_rejected( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + mocker.patch.object(connection, "enter_navigation_mode", return_value=False) + ensure_rl_control = mocker.patch.object(connection, "_ensure_rl_control") + assert connection.standup() is False -def test_connection_refuses_arm_without_a_fresh_lidar_stream( + ensure_rl_control.assert_not_called() + + +def test_navigation_mode_uses_documented_basic_server_apdu( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + response_payload = json.dumps( + { + "PatrolDevice": { + "Type": 1101, + "Command": 5, + "Items": {"ErrorCode": 0}, + } + } + ).encode() + response_header = struct.pack( + "<4sHHB7s", + bytes.fromhex("eb91eb90"), + len(response_payload), + 0, + 1, + b"\0" * 7, + ) + basic_server_socket = mocker.MagicMock() + basic_server_socket.__enter__.return_value = basic_server_socket + basic_server_socket.recv.side_effect = [response_header, response_payload] + create_connection = mocker.patch( + "dimos.robot.deeprobotics.m20.connection.socket.create_connection", + return_value=basic_server_socket, + ) + + assert connection.enter_navigation_mode() is True + + create_connection.assert_called_once_with(("10.21.31.103", 30001), timeout=3.0) + sent = basic_server_socket.sendall.call_args.args[0] + magic, payload_length, message_id, encoding, reserved = struct.unpack("<4sHHB7s", sent[:16]) + request = json.loads(sent[16:]) + assert (magic, payload_length, message_id, encoding, reserved) == ( + bytes.fromhex("eb91eb90"), + len(sent) - 16, + 0, + 1, + b"\0" * 7, + ) + assert request["PatrolDevice"]["Type"] == 1101 + assert request["PatrolDevice"]["Command"] == 5 + assert request["PatrolDevice"]["Items"] == {"Mode": 1} + + +def test_low_level_motion_and_gait_endpoints_publish_vendor_commands( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + motion_publish = mocker.patch.object(connection.motion_state_cmd, "publish") + gait_publish = mocker.patch.object(connection.gait_cmd, "publish") + wait = mocker.patch.object(connection, "_wait_for_motion_state", return_value=True) + + assert connection.enter_rl_control() is True + assert connection.set_navigation_gait() is True + assert connection.liedown() is True + + assert [call.args[0].data for call in motion_publish.call_args_list] == [17, 4] + assert gait_publish.call_args.args[0].data == GAIT_FLAT_AGILE + wait.assert_called_once_with(17, 5.0) + + +def test_rl_control_transition_follows_vendor_stand_sequence( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + motion_publish = mocker.patch.object(connection.motion_state_cmd, "publish") + wait = mocker.patch.object(connection, "_wait_for_motion_state", return_value=True) + + assert connection._ensure_rl_control() is True + + assert [call.args[0].data for call in motion_publish.call_args_list] == [1, 17] + assert wait.call_args_list == [mocker.call(1, 12.0), mocker.call(17, 5.0)] + + +def test_rejects_unknown_gait( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + publish = mocker.patch.object(connection.gait_cmd, "publish") + + assert connection.set_gait(12345) is False + publish.assert_not_called() + + +def test_lidar_diagnostics_do_not_gate_robot_control( mocker: MockerFixture, connection_factory: Callable[..., M20Connection], ) -> None: connection = connection_factory() armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_localization_ready(Bool(True)) connection._on_command_ready(Bool(True)) accepted = connection.arm() - assert accepted is False + assert accepted is True assert connection.is_lidar_ready() is False - assert connection.is_armed() is False - armed_publish.assert_not_called() + assert connection.is_armed() is True + armed_publish.assert_called_once() def test_connection_reports_lidar_recovery( @@ -203,7 +341,7 @@ def test_connection_reports_lidar_recovery( assert connection.is_lidar_ready() is True -def test_connection_disarms_when_lidar_stream_becomes_stale( +def test_lidar_staleness_does_not_clear_operator_arm( mocker: MockerFixture, connection_factory: Callable[..., M20Connection], ) -> None: @@ -211,6 +349,7 @@ def test_connection_disarms_when_lidar_stream_becomes_stale( safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") armed_publish = mocker.patch.object(connection.armed, "publish") connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) connection._on_command_ready(Bool(True)) connection.arm() safe_publish.reset_mock() @@ -219,7 +358,44 @@ def test_connection_disarms_when_lidar_stream_becomes_stale( connection._on_lidar_ready(Bool(False)) assert connection.is_lidar_ready() is False - assert connection.is_armed() is False - safe_publish.assert_called_once_with(Twist.zero()) + assert connection.is_armed() is True + safe_publish.assert_not_called() + armed_publish.assert_not_called() + + +def test_pointlio_diagnostics_do_not_gate_robot_control( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) + connection._on_command_ready(Bool(True)) + + accepted = connection.arm() + + assert accepted is True + assert connection.is_localization_ready() is False armed_publish.assert_called_once() - assert armed_publish.call_args.args[0].data is False + + +def test_pointlio_staleness_does_not_clear_operator_arm( + mocker: MockerFixture, + connection_factory: Callable[..., M20Connection], +) -> None: + connection = connection_factory() + safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") + armed_publish = mocker.patch.object(connection.armed, "publish") + connection._on_lidar_ready(Bool(True)) + connection._on_localization_ready(Bool(True)) + connection._on_command_ready(Bool(True)) + assert connection.arm() is True + safe_publish.reset_mock() + armed_publish.reset_mock() + + connection._on_localization_ready(Bool(False)) + + assert connection.is_localization_ready() is False + assert connection.is_armed() is True + safe_publish.assert_not_called() + armed_publish.assert_not_called() From 45633e3af6fde0894fbb89c217eb829aab22ffda Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 17:13:59 +0800 Subject: [PATCH 04/15] perf(robot): ingest M20 lidar directly in Point-LIO --- .../m20/blueprints/m20_kronknav.py | 9 +- .../m20/blueprints/test_m20_kronknav.py | 19 +- dimos/robot/deeprobotics/m20/bridge/README.md | 93 ++---- .../m20/bridge/cpp/CMakeLists.txt | 4 - .../deeprobotics/m20/bridge/cpp/main.cpp | 276 +----------------- dimos/robot/deeprobotics/m20/bridge/module.py | 22 +- .../deeprobotics/m20/bridge/test_module.py | 37 --- dimos/robot/deeprobotics/m20/deploy/README.md | 19 +- .../m20/pointlio/cpp/CMakeLists.txt | 41 ++- .../deeprobotics/m20/pointlio/cpp/build.sh | 16 + .../m20/pointlio/cpp/compat/glog/logging.h | 8 - .../deeprobotics/m20/pointlio/cpp/main.cpp | 177 ++++++++--- .../m20/pointlio/cpp/pointlio-gos.patch | 16 + .../robot/deeprobotics/m20/pointlio/module.py | 26 +- 14 files changed, 268 insertions(+), 495 deletions(-) delete mode 100644 dimos/robot/deeprobotics/m20/bridge/test_module.py delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h create mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 69a59fff4e..0537e46bdd 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -101,11 +101,7 @@ def _m20_rerun_blueprint() -> Any: "world/global_map": 0, }, "visual_override": { - # These are internal high-rate bridge streams. Logging the 100k-point - # raw cloud and 200 Hz IMU saturated the RK3588 and queued minutes of - # Rerun data. The Go2 navigation view likewise shows maps, not lidar. - "world/raw_lidar": None, - "world/imu": None, + # The navigation view shows maps rather than the registered lidar. "world/lidar": None, "world/global_map": _render_global_map, "world/planner_path": None, @@ -118,12 +114,11 @@ def _m20_rerun_blueprint() -> Any: } -# Safe hardware bring-up graph: raw M20 sensors -> native Point-LIO -> Rerun. +# Safe hardware bring-up graph: direct ROS sensors -> native Point-LIO -> Rerun. # It intentionally has no connection/controller modules and cannot publish # /NAV_CMD. Use this before starting the full mapper/planner blueprint. deeprobotics_m20_pointlio = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), - M20ROSBridge.blueprint(enable_command_output=False), M20PointLio.blueprint(), ).global_config(n_workers=2, transport="lcm") diff --git a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py index 865ce6205b..e01f9e6c16 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py @@ -20,7 +20,6 @@ from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.tf2_msgs.TFMessage import TFMessage @@ -77,7 +76,7 @@ def test_default_kronknav_blueprint_cannot_publish_robot_commands() -> None: def test_pointlio_bringup_blueprint_has_no_command_publisher() -> None: - assert _bridge_kwargs(deeprobotics_m20_pointlio)["enable_command_output"] is False + assert not any(atom.module is M20ROSBridge for atom in deeprobotics_m20_pointlio.blueprints) assert not any(atom.module is M20Connection for atom in deeprobotics_m20_pointlio.blueprints) @@ -93,19 +92,13 @@ def test_m20_blueprints_pin_native_sdk_supported_local_transport() -> None: def test_sensor_and_pose_streams_reach_mapping_and_navigation() -> None: blueprint = deeprobotics_m20_kronknav - assert _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "out") == {M20ROSBridge} - assert _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "in") == {M20PointLio} - assert _endpoint_modules(blueprint, "imu", Imu, "out") == {M20ROSBridge} - assert _endpoint_modules(blueprint, "imu", Imu, "in") == {M20PointLio} + assert not _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "out") assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20PointLio} assert RayTracingVoxelMap in _endpoint_modules(blueprint, "lidar", PointCloud2, "in") - assert _endpoint_modules(blueprint, "lidar_ready", Bool, "out") == {M20ROSBridge} + assert _endpoint_modules(blueprint, "lidar_ready", Bool, "out") == {M20PointLio} assert _endpoint_modules(blueprint, "lidar_ready", Bool, "in") == {M20Connection} assert _endpoint_modules(blueprint, "localization_ready", Bool, "out") == {M20PointLio} - assert _endpoint_modules(blueprint, "localization_ready", Bool, "in") == { - M20ROSBridge, - M20Connection, - } + assert _endpoint_modules(blueprint, "localization_ready", Bool, "in") == {M20Connection} assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20PointLio} assert _endpoint_modules(blueprint, "tf", TFMessage, "in") == { RayTracingVoxelMap, @@ -158,8 +151,8 @@ def test_rerun_uses_go2_navigation_data_budget() -> None: max_hz = config["max_hz"] assert isinstance(visual_override, dict) - assert visual_override["world/raw_lidar"] is None - assert visual_override["world/imu"] is None + assert "world/raw_lidar" not in visual_override + assert "world/imu" not in visual_override assert visual_override["world/lidar"] is None assert isinstance(max_hz, dict) assert max_hz["world/local_map"] == 0.5 diff --git a/dimos/robot/deeprobotics/m20/bridge/README.md b/dimos/robot/deeprobotics/m20/bridge/README.md index 83061fcbd9..51a009d2df 100644 --- a/dimos/robot/deeprobotics/m20/bridge/README.md +++ b/dimos/robot/deeprobotics/m20/bridge/README.md @@ -1,70 +1,23 @@ -# M20 robot-local ROS bridge +# M20 command/state ROS bridge -This native module is built and run on the M20 Pro GOS computer. It converts the -vendor's local ROS 2/DrDDS topics into typed DimOS streams without a Python -`rclpy` dependency or another application host. +This native module runs on the M20 Pro GOS computer and links against the +installed ROS 2 Foxy and `drdds` packages. It carries only low-bandwidth robot +command and state traffic between ROS and typed DimOS streams. -The current C++ NativeModule SDK carries those streams over local LCM. The M20 -blueprints therefore pin the complete onboard graph to LCM; this bridge does not -contain a private Zenoh implementation. +Lidar and IMU do not pass through this bridge. `M20PointLio` subscribes directly +to `/LIDAR/POINTS` and `/IMU` in its own native process, validates the X20 cloud +contract, converts the IMU units, and publishes `lidar_ready`, localization, +odometry, TF, and the map-ready cloud. Keeping the sensor payload out of this +bridge avoids copying and serializing every 0.8-2.1 MB cloud through LCM. -The bridge always subscribes to `/LIDAR/POINTS`, `/IMU`, and `/HES_STATUS`. It -publishes the raw cloud, IMU, lidar-readiness, and command-readiness streams -into the local DimOS graph. `M20PointLio` consumes the cloud and IMU and is the -sole producer of pose, odometry, TF, map-ready lidar, and localization-readiness. +The bridge subscribes to the vendor motion and hard-estop state topics. When +`enable_command_output` is explicitly enabled, it owns the `/NAV_CMD`, +`/MOTION_STATE`, and `/GAIT` publishers. Startup does not change the robot mode, +gait, charging state, or standing state. -The inspected M20 publishes merged front/rear clouds at 10 Hz with reliable, -volatile DDS QoS. Each point uses the vendor's 26-byte layout (`x`, `y`, `z`, -`intensity`, `ring`, `timestamp`); the bridge preserves the fields and bytes. -Before forwarding, it rejects empty, malformed, big-endian, or XYZ-less clouds. -The native steady-clock watchdog marks the stream stale after 0.5 seconds -without a mapper-compatible cloud and logs every loss/recovery transition. The -vendor driver reports `lidar_link`, although its documented merge already -applies both sensor extrinsics into `base_link`, so the bridge normalizes that -known-mislabeled frame to `base_link`. - -GOS starts `rsdriver.service` as root and creates its Fast DDS shared-memory -segment with mode `0644`. A normal `user` subscriber cannot attach, and the -vendor writer does not fall back to UDP for a same-host cloud reader. Install -the checked-in `deploy/rsdriver.service.d` drop-in and its permission helper. -The service stays root for real-time scheduling, but its active Fast DDS files -become group-writable by the existing `user` group so DimOS remains unprivileged. - -`enable_command_output` defaults to `false`. When explicitly enabled, the bridge -owns a `/NAV_CMD` publisher but emits nonzero velocity only while: - -- the local PointLIO estimate is fresh and advancing; -- `/MOTION_INFO` is fresh and confirms RL Control state `17`; -- any received hard-estop status is exactly `0` (not triggered); -- a valid merged lidar cloud has arrived within the lidar timeout; -- the `/NAV_CMD` publisher has a matched subscriber; -- the Python connection has explicitly armed and supplied a fresh bounded command. - -The inspected firmware advertises `/HES_STATUS` with the documented DDS type -and QoS but emits no samples, including to the vendor `ros2 topic echo` tool. -The bridge therefore does not misuse it as a liveness heartbeat: an observed -trigger still vetoes commands, while the robot controller independently -enforces the physical hard stop below this API. - -The native watchdog uses a steady clock and sends zero after command timeout, -on robot-control loss, and during shutdown. Lidar and PointLIO readiness remain -navigation diagnostics; they do not permanently disarm Go2-style manual motion. -Starting the bridge never changes robot mode, gait, planner service, charging -state, or standing state on startup. The normal explicit operator action is one -`M20Connection.standup()` RPC, which switches `basic_server` to navigation usage -mode, completes Stand → RL Control, resets and selects the navigation gait, -waits for command-path feedback, and arms. - -The bridge diagnoses but does not remotely manage the vendor sensor pipeline. -For boot-persistent clouds, `multicast-relay.service` must be enabled on NOS and -`rsdriver.service` enabled on GOS; both must be active before DimOS starts. -Install the GOS service drop-in/helper from `deploy/`, and load -`deploy/dimos-m20.env` in the DimOS launcher. The explicit 16 MiB LCM receive -buffer is required for the observed 0.8-2.1 MB clouds; the LCM bus keeps its -default TTL 0, so this high-bandwidth stream remains local to GOS. - -See [`deploy/README.md`](../deploy/README.md) for the complete packet path, -persistent installation steps, live checks, and remote Rerun attachment. +The current C++ NativeModule SDK carries DimOS command/state streams over local +LCM. The M20 blueprints therefore pin the onboard graph to LCM; this bridge does +not contain a private Zenoh implementation. Build on GOS after sourcing the vendor environment: @@ -72,12 +25,10 @@ Build on GOS after sourcing the vendor environment: ./build.sh ``` -Deploy from a DimOS source checkout. The Python wheel does not include this C++ -source tree or the in-repo native SDK, both of which the robot-local build uses. - The default setup path is `/opt/robot/scripts/setup_ros2.sh`; override it with -`M20_ROS_SETUP` if the inspected robot differs. The build intentionally fails -off-robot when Foxy and the installed `drdds` message package are unavailable. -It also requires CMake, a C++20 compiler, pkg-config, and the LCM development -package. For an offline build, clone the pinned `dimos-lcm` revision separately -and set `DIMOS_LCM_DIR` to that checkout before running `build.sh`. +`M20_ROS_SETUP` if necessary. The build intentionally fails off-robot when +Foxy and the installed `drdds` package are unavailable. For an offline build, +set `DIMOS_LCM_DIR` to the pinned `dimos-lcm` checkout. + +See [`deploy/README.md`](../deploy/README.md) for the robot service setup, +command ownership, and launch procedure. diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt index 23a3611546..ad8769059c 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt @@ -10,9 +10,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) find_package(ament_cmake REQUIRED) find_package(drdds REQUIRED) -find_package(nav_msgs REQUIRED) find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) @@ -43,9 +41,7 @@ target_include_directories(m20_ros_bridge PRIVATE ) ament_target_dependencies(m20_ros_bridge drdds - nav_msgs rclcpp - sensor_msgs ) target_link_libraries(m20_ros_bridge dimos_native diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp index e252745a98..9b739fc333 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp @@ -1,17 +1,14 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 // -// Robot-local ROS 2/DrDDS adapter for the Deep Robotics Lynx M20. -// DimOS and this process both run on GOS. ROS 2 is used only to reach the -// vendor topics; typed DimOS streams use the native SDK's local LCM transport. +// Robot-local command/state ROS 2/DrDDS adapter for the Deep Robotics Lynx M20. +// High-bandwidth lidar and IMU ingress belongs to the M20 Point-LIO process. #include #include #include #include #include -#include -#include #include #include #include @@ -26,18 +23,11 @@ #include #include #include -#include -#include -#include #include "dimos/native.hpp" #include "geometry_msgs/Twist.hpp" -#include "sensor_msgs/Imu.hpp" -#include "sensor_msgs/PointCloud2.hpp" -#include "sensor_msgs/PointField.hpp" #include "std_msgs/Bool.hpp" -#include "std_msgs/Header.hpp" #include "std_msgs/Int32.hpp" #include "std_msgs/UInt32.hpp" @@ -59,98 +49,6 @@ void require_nonempty(const std::string& value, const char* name) { } } -int32_t checked_i32(std::size_t value, const char* name) { - if (value > static_cast(std::numeric_limits::max())) { - throw std::runtime_error(std::string(name) + " exceeds the DimOS message limit"); - } - return static_cast(value); -} - -std::size_t checked_product(std::size_t left, std::size_t right, const char* name) { - if (left != 0 && right > std::numeric_limits::max() / left) { - throw std::runtime_error(std::string(name) + " overflows size_t"); - } - return left * right; -} - -struct XYZOffsets { - std::size_t x; - std::size_t y; - std::size_t z; -}; - -XYZOffsets validate_cloud_for_mapping(const sensor_msgs::msg::PointCloud2& cloud) { - if (cloud.width == 0 || cloud.height == 0) { - throw std::runtime_error("point cloud is empty"); - } - if (cloud.is_bigendian) { - throw std::runtime_error("big-endian point clouds are not supported by the mapper"); - } - if (cloud.point_step == 0) { - throw std::runtime_error("point-cloud point_step is zero"); - } - if (cloud.point_step < sizeof(float)) { - throw std::runtime_error("point-cloud point_step is shorter than float32"); - } - - const auto row_bytes = checked_product(static_cast(cloud.width), - static_cast(cloud.point_step), - "point-cloud row size"); - if (cloud.row_step != row_bytes) { - throw std::runtime_error("point cloud contains unsupported row padding"); - } - const auto required_bytes = checked_product(row_bytes, static_cast(cloud.height), - "point-cloud byte count"); - if (cloud.data.size() < required_bytes) { - throw std::runtime_error("point-cloud data is shorter than its dimensions"); - } - - XYZOffsets offsets{std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max()}; - for (const auto& field : cloud.fields) { - if (field.datatype != sensor_msgs::msg::PointField::FLOAT32 || field.count == 0) { - continue; - } - const auto offset = static_cast(field.offset); - if (offset > static_cast(cloud.point_step) - sizeof(float)) { - continue; - } - if (field.name == "x") { - offsets.x = offset; - } else if (field.name == "y") { - offsets.y = offset; - } else if (field.name == "z") { - offsets.z = offset; - } - } - const auto missing = std::numeric_limits::max(); - if (offsets.x == missing || offsets.y == missing || offsets.z == missing) { - throw std::runtime_error("point cloud lacks mapper-compatible float32 x/y/z fields"); - } - return offsets; -} - -bool has_finite_xyz(const sensor_msgs::msg::PointCloud2& cloud, const XYZOffsets& offsets) { - const auto point_count = checked_product(static_cast(cloud.width), - static_cast(cloud.height), - "point-cloud point count"); - const auto point_step = static_cast(cloud.point_step); - for (std::size_t index = 0; index < point_count; ++index) { - const auto base = index * point_step; - float x = 0.0F; - float y = 0.0F; - float z = 0.0F; - std::memcpy(&x, cloud.data.data() + base + offsets.x, sizeof(float)); - std::memcpy(&y, cloud.data.data() + base + offsets.y, sizeof(float)); - std::memcpy(&z, cloud.data.data() + base + offsets.z, sizeof(float)); - if (std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) { - return true; - } - } - return false; -} - double clamp(double value, double limit) { return std::max(-limit, std::min(limit, value)); } @@ -249,54 +147,33 @@ int vendor_int32_value(const Status& status) { } } -std_msgs::Header to_dimos_header(const std_msgs::msg::Header& source, - const std::string& frame_id) { - static std::atomic sequence{0}; - std_msgs::Header result; - result.seq = sequence.fetch_add(1, std::memory_order_relaxed); - result.stamp.sec = source.stamp.sec; - result.stamp.nsec = static_cast(source.stamp.nanosec); - result.frame_id = frame_id; - return result; -} - } // namespace struct M20ROSBridgeConfig { - std::string lidar_topic; - std::string imu_topic; std::string nav_cmd_topic; std::string motion_state_topic; std::string motion_info_topic; std::string gait_topic; std::string hes_status_topic; std::string node_name; - std::string cloud_frame; - std::string base_frame; bool enable_command_output; double command_rate_hz; double command_timeout_s; double safety_timeout_s; - double lidar_timeout_s; double max_linear_x; double max_linear_y; double max_angular_z; void validate() const { - require_nonempty(lidar_topic, "lidar_topic"); - require_nonempty(imu_topic, "imu_topic"); require_nonempty(nav_cmd_topic, "nav_cmd_topic"); require_nonempty(motion_state_topic, "motion_state_topic"); require_nonempty(motion_info_topic, "motion_info_topic"); require_nonempty(gait_topic, "gait_topic"); require_nonempty(hes_status_topic, "hes_status_topic"); require_nonempty(node_name, "node_name"); - require_nonempty(cloud_frame, "cloud_frame"); - require_nonempty(base_frame, "base_frame"); dimos::native::require_positive(command_rate_hz, "command_rate_hz"); dimos::native::require_positive(command_timeout_s, "command_timeout_s"); dimos::native::require_positive(safety_timeout_s, "safety_timeout_s"); - dimos::native::require_positive(lidar_timeout_s, "lidar_timeout_s"); dimos::native::require_positive(max_linear_x, "max_linear_x"); dimos::native::require_positive(max_linear_y, "max_linear_y"); dimos::native::require_positive(max_angular_z, "max_angular_z"); @@ -305,21 +182,16 @@ struct M20ROSBridgeConfig { M20ROSBridgeConfig parse_m20_config(Config& config) { M20ROSBridgeConfig result{}; - result.lidar_topic = config.take("lidar_topic"); - result.imu_topic = config.take("imu_topic"); result.nav_cmd_topic = config.take("nav_cmd_topic"); result.motion_state_topic = config.take("motion_state_topic"); result.motion_info_topic = config.take("motion_info_topic"); result.gait_topic = config.take("gait_topic"); result.hes_status_topic = config.take("hes_status_topic"); result.node_name = config.take("node_name"); - result.cloud_frame = config.take("cloud_frame"); - result.base_frame = config.take("base_frame"); result.enable_command_output = config.take("enable_command_output"); result.command_rate_hz = config.take("command_rate_hz"); result.command_timeout_s = config.take("command_timeout_s"); result.safety_timeout_s = config.take("safety_timeout_s"); - result.lidar_timeout_s = config.take("lidar_timeout_s"); result.max_linear_x = config.take("max_linear_x"); result.max_linear_y = config.take("max_linear_y"); result.max_angular_z = config.take("max_angular_z"); @@ -333,17 +205,12 @@ class M20ROSBridge : public Module { void build(Builder& builder, Config& config) override { cfg_ = parse_m20_config(config); builder.input("safe_cmd_vel", &M20ROSBridge::on_command, this); - builder.input("localization_ready", - &M20ROSBridge::on_localization_ready, this); builder.input("motion_state_cmd", &M20ROSBridge::on_motion_state_command, this); builder.input("gait_cmd", &M20ROSBridge::on_gait_command, this); command_ready_ = builder.output("command_ready"); - lidar_ready_ = builder.output("lidar_ready"); motion_state_ = builder.output("motion_state"); gait_state_ = builder.output("gait_state"); - raw_lidar_ = builder.output("raw_lidar"); - imu_ = builder.output("imu"); } void setup() override { @@ -354,11 +221,6 @@ class M20ROSBridge : public Module { dimos::native::install_signal_handlers(); node_ = std::make_shared(cfg_.node_name); - // Both inspected bare-DDS M20 publishers offer RELIABLE/VOLATILE. Pin - // the cloud subscription to that verified contract so DDS detects a - // future incompatible vendor QoS change instead of silently dropping. - const auto lidar_qos = - rclcpp::QoS(rclcpp::KeepLast(2)).reliable().durability_volatile(); // The advertised M20 endpoint is RELIABLE/TRANSIENT_LOCAL. Some M20 // firmware revisions expose the endpoint without actually emitting // its documented 1 Hz samples, so HES is a veto when observed rather @@ -366,13 +228,6 @@ class M20ROSBridge : public Module { // below this API by the robot controller. const auto hes_qos = rclcpp::QoS(rclcpp::KeepLast(2)).reliable().transient_local(); - lidar_subscription_ = node_->create_subscription( - cfg_.lidar_topic, lidar_qos, - [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { on_lidar(*msg); }); - imu_subscription_ = node_->create_subscription( - cfg_.imu_topic, - rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(), - [this](sensor_msgs::msg::Imu::SharedPtr msg) { on_imu(*msg); }); hes_subscription_ = node_->create_subscription( cfg_.hes_status_topic, hes_qos, [this](drdds::msg::StdMsgInt32::SharedPtr msg) { on_hes_status(*msg); }); @@ -399,11 +254,8 @@ class M20ROSBridge : public Module { executor_->add_node(node_); spin_thread_ = std::thread([this]() { executor_->spin(); }); - logging::info( - "M20 ROS bridge started", - {logging::Field("lidar_topic", cfg_.lidar_topic), - logging::Field("imu_topic", cfg_.imu_topic), - logging::Field("command_output", cfg_.enable_command_output)}); + logging::info("M20 command/state ROS bridge started", + {logging::Field("command_output", cfg_.enable_command_output)}); } void teardown() override { @@ -425,8 +277,6 @@ class M20ROSBridge : public Module { gait_publisher_.reset(); hes_subscription_.reset(); motion_info_subscription_.reset(); - imu_subscription_.reset(); - lidar_subscription_.reset(); if (executor_ != nullptr && node_ != nullptr) { executor_->remove_node(node_); } @@ -438,68 +288,6 @@ class M20ROSBridge : public Module { } private: - void on_lidar(const sensor_msgs::msg::PointCloud2& source) { - try { - const XYZOffsets offsets = validate_cloud_for_mapping(source); - if (!has_finite_xyz(source, offsets)) { - throw std::runtime_error("point cloud contains no finite XYZ return"); - } - sensor_msgs::PointCloud2 result; - result.header = to_dimos_header(source.header, cfg_.cloud_frame); - result.height = checked_i32(source.height, "point-cloud height"); - result.width = checked_i32(source.width, "point-cloud width"); - result.fields_length = checked_i32(source.fields.size(), "point-cloud field count"); - result.fields.reserve(source.fields.size()); - for (const auto& source_field : source.fields) { - sensor_msgs::PointField field; - field.name = source_field.name; - field.offset = checked_i32(source_field.offset, "point-field offset"); - field.datatype = source_field.datatype; - field.count = checked_i32(source_field.count, "point-field count"); - result.fields.push_back(std::move(field)); - } - result.is_bigendian = static_cast(source.is_bigendian); - result.point_step = checked_i32(source.point_step, "point-cloud point step"); - result.row_step = checked_i32(source.row_step, "point-cloud row step"); - result.data_length = checked_i32(source.data.size(), "point-cloud byte count"); - result.data = source.data; - result.is_dense = static_cast(source.is_dense); - raw_lidar_.publish(result); - { - std::lock_guard lock(state_mutex_); - lidar_received_at_ = Clock::now(); - last_lidar_width_ = source.width; - have_lidar_ = true; - } - } catch (const std::exception& error) { - logging::error("dropping invalid M20 point cloud", - {logging::Field("error", std::string(error.what()))}); - } - } - - void on_imu(const sensor_msgs::msg::Imu& source) { - sensor_msgs::Imu result; - result.header = to_dimos_header(source.header, cfg_.base_frame); - result.orientation.x = source.orientation.x; - result.orientation.y = source.orientation.y; - result.orientation.z = source.orientation.z; - result.orientation.w = source.orientation.w; - result.angular_velocity.x = source.angular_velocity.x; - result.angular_velocity.y = source.angular_velocity.y; - result.angular_velocity.z = source.angular_velocity.z; - result.linear_acceleration.x = source.linear_acceleration.x; - result.linear_acceleration.y = source.linear_acceleration.y; - result.linear_acceleration.z = source.linear_acceleration.z; - for (std::size_t index = 0; index < 9; ++index) { - result.orientation_covariance[index] = source.orientation_covariance[index]; - result.angular_velocity_covariance[index] = - source.angular_velocity_covariance[index]; - result.linear_acceleration_covariance[index] = - source.linear_acceleration_covariance[index]; - } - imu_.publish(result); - } - void on_command(const geometry_msgs::Twist& source) { geometry_msgs::Twist bounded = zero_twist(); if (std::isfinite(source.linear.x) && std::isfinite(source.linear.y) && @@ -514,13 +302,6 @@ class M20ROSBridge : public Module { have_command_ = true; } - void on_localization_ready(const std_msgs::Bool& source) { - std::lock_guard lock(state_mutex_); - localization_ready_state_ = source.data != 0; - localization_received_at_ = Clock::now(); - have_localization_ = true; - } - void on_motion_state_command(const std_msgs::Int32& source) { if (motion_state_publisher_ == nullptr || !rclcpp::ok()) return; drdds::msg::MotionState output; @@ -574,21 +355,6 @@ class M20ROSBridge : public Module { (!have_hes_ || hes_status_ == 0); } - bool lidar_fresh(Clock::time_point now) const { - std::lock_guard lock(state_mutex_); - const auto timeout = std::chrono::duration(cfg_.lidar_timeout_s); - return have_lidar_ && now - lidar_received_at_ <= timeout; - } - - std::pair lidar_diagnostics(Clock::time_point now) const { - std::lock_guard lock(state_mutex_); - if (!have_lidar_) { - return {-1.0, 0}; - } - return {std::chrono::duration(now - lidar_received_at_).count(), - last_lidar_width_}; - } - geometry_msgs::Twist fresh_command_or_zero(Clock::time_point now) const { std::lock_guard lock(state_mutex_); const auto timeout = std::chrono::duration(cfg_.command_timeout_s); @@ -600,28 +366,6 @@ class M20ROSBridge : public Module { void publish_cycle(bool force_zero) { const auto now = Clock::now(); - const bool cloud_ready = !force_zero && !stopping_.load(std::memory_order_acquire) && - lidar_fresh(now); - std_msgs::Bool lidar_ready_message; - lidar_ready_message.data = static_cast(cloud_ready); - lidar_ready_.publish(lidar_ready_message); - - const int8_t new_lidar_state = cloud_ready ? 1 : 0; - const int8_t previous_lidar_state = - lidar_health_state_.exchange(new_lidar_state, std::memory_order_acq_rel); - if (previous_lidar_state != new_lidar_state) { - const auto [age_s, width] = lidar_diagnostics(now); - if (cloud_ready) { - logging::info("M20 lidar stream is healthy", - {logging::Field("cloud_age_s", age_s), - logging::Field("cloud_width", static_cast(width))}); - } else { - logging::warn("M20 lidar stream is missing or stale", - {logging::Field("cloud_age_s", age_s), - logging::Field("timeout_s", cfg_.lidar_timeout_s)}); - } - } - const bool ready = !force_zero && !stopping_.load(std::memory_order_acquire) && nav_cmd_publisher_ != nullptr && nav_cmd_publisher_->get_subscription_count() > 0 && @@ -644,16 +388,11 @@ class M20ROSBridge : public Module { M20ROSBridgeConfig cfg_; Output command_ready_; - Output lidar_ready_; Output motion_state_; Output gait_state_; - Output raw_lidar_; - Output imu_; std::shared_ptr node_; std::shared_ptr executor_; - rclcpp::Subscription::SharedPtr lidar_subscription_; - rclcpp::Subscription::SharedPtr imu_subscription_; rclcpp::Subscription::SharedPtr hes_subscription_; rclcpp::Subscription::SharedPtr motion_info_subscription_; rclcpp::Publisher::SharedPtr nav_cmd_publisher_; @@ -665,20 +404,13 @@ class M20ROSBridge : public Module { mutable std::mutex state_mutex_; geometry_msgs::Twist latest_command_ = zero_twist(); Clock::time_point command_received_at_{}; - Clock::time_point localization_received_at_{}; Clock::time_point motion_info_received_at_{}; - Clock::time_point lidar_received_at_{}; bool have_command_ = false; - bool have_localization_ = false; bool have_hes_ = false; bool have_motion_info_ = false; - bool have_lidar_ = false; - bool localization_ready_state_ = false; - uint32_t last_lidar_width_ = 0; int motion_state_value_ = 0; int hes_status_ = 1; std::atomic stopping_{false}; - std::atomic lidar_health_state_{-1}; std::atomic command_sequence_{0}; }; diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py index 74ec26902f..bda15cfac2 100644 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""NativeModule declaration for the M20's local ROS 2/DrDDS adapter.""" +"""NativeModule declaration for the M20 command/state ROS 2 adapter.""" from __future__ import annotations @@ -23,8 +23,6 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.sensor_msgs.Imu import Imu -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.std_msgs.Int32 import Int32 from dimos.msgs.std_msgs.UInt32 import UInt32 @@ -36,7 +34,7 @@ class M20ROSBridgeConfig(NativeModuleConfig): - """Robot-local ROS topic names, frames, and command watchdog settings.""" + """Robot-local command/state topics and watchdog settings.""" cwd: str | None = "cpp" executable: str = "build/m20_ros_bridge" @@ -52,8 +50,6 @@ class M20ROSBridgeConfig(NativeModuleConfig): } ) - lidar_topic: str = "/LIDAR/POINTS" - imu_topic: str = "/IMU" nav_cmd_topic: str = "/NAV_CMD" motion_state_topic: str = "/MOTION_STATE" motion_info_topic: str = "/MOTION_INFO" @@ -61,39 +57,31 @@ class M20ROSBridgeConfig(NativeModuleConfig): hes_status_topic: str = "/HES_STATUS" node_name: str = "dimos_m20_bridge" - cloud_frame: str = "base_link" - base_frame: str = "base_link" - enable_command_output: bool = False command_rate_hz: float = Field(default=10.0, gt=0.0) command_timeout_s: float = Field(default=0.4, gt=0.0) safety_timeout_s: float = Field(default=2.5, gt=0.0) - lidar_timeout_s: float = Field(default=0.5, gt=0.0) max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) class M20ROSBridge(NativeModule): - """Bridge only M20 raw sensors and guarded commands to typed DimOS streams. + """Bridge M20 command and state topics to typed DimOS streams. This process runs on GOS and links against the robot's installed Foxy and - ``drdds`` packages. It has no vendor odometry input: M20PointLio owns the - odometry estimate. It does not route through Zenoh or another robot host. + ``drdds`` packages. M20PointLio subscribes to lidar and IMU directly; this + bridge carries only low-bandwidth command and robot-state traffic. """ config: M20ROSBridgeConfig safe_cmd_vel: In[Twist] - localization_ready: In[Bool] motion_state_cmd: In[Int32] gait_cmd: In[UInt32] command_ready: Out[Bool] - lidar_ready: Out[Bool] motion_state: Out[Int32] gait_state: Out[UInt32] - raw_lidar: Out[PointCloud2] - imu: Out[Imu] if TYPE_CHECKING: diff --git a/dimos/robot/deeprobotics/m20/bridge/test_module.py b/dimos/robot/deeprobotics/m20/bridge/test_module.py deleted file mode 100644 index 8437f536e4..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/test_module.py +++ /dev/null @@ -1,37 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Configuration contract tests for the native M20 ROS bridge.""" - -from pydantic import ValidationError -import pytest - -from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridgeConfig - - -def test_bridge_requires_five_missed_nominal_clouds_before_stale() -> None: - config = M20ROSBridgeConfig() - - assert config.lidar_timeout_s == 0.5 - - -def test_bridge_normalizes_the_vendor_mislabeled_cloud_frame() -> None: - config = M20ROSBridgeConfig() - - assert config.cloud_frame == "base_link" - - -def test_bridge_rejects_nonpositive_lidar_timeout() -> None: - with pytest.raises(ValidationError): - M20ROSBridgeConfig(lidar_timeout_s=0.0) diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index dc69297101..3e78b795d7 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -7,8 +7,6 @@ front lidar 10.21.33.201 -- MSOP 6691 / DIFOP 7781 --+ +--> NOS multicast-relay.service rear lidar 10.21.33.202 -- MSOP 6692 / DIFOP 7782 --+ --> GOS rsdriver.service --> DDS /LIDAR/POINTS - --> m20_ros_bridge - --> local LCM raw lidar + IMU --> M20PointLio --> RayTracingVoxelMap ``` @@ -142,10 +140,10 @@ module, LCM traffic, ROS/DDS traffic, or Zenoh router runs on AOS. ## Run DimOS locally on GOS -Load the local-only LCM URL before launching DimOS. Its explicit 16 MiB receive -buffer is needed for fragmented multi-megabyte clouds. The native ROS bridge -also needs the vendor Foxy library path and Fast DDS profile in its inherited -environment: +Load the local-only LCM URL before launching DimOS. Raw lidar and IMU remain in +Fast DDS and enter `M20PointLio` directly; they are not serialized through LCM. +Both native ROS processes need the vendor Foxy library path and Fast DDS profile +in their inherited environment: ```bash source /opt/ros/foxy/setup.bash @@ -197,13 +195,10 @@ source /opt/robot/scripts/setup_ros2.sh ros2 topic hz --wall-time --window 100 /LIDAR/POINTS ``` -The native bridge additionally validates every cloud and publishes +The native `M20PointLio` process validates every cloud and publishes `lidar_ready` at 10 Hz. Five missed nominal frames (0.5 seconds) make it false. -The bridge logs `M20 lidar stream is missing or stale` once on loss and -`M20 lidar stream is healthy` once on recovery. Both the native `/NAV_CMD` -watchdog and `M20Connection` require fresh lidar, so loss immediately forces -zero velocity and disarms the Python gate. DDS rematches automatically after an -`rsdriver.service` restart; DimOS does not need to restart. +DDS rematches automatically after an `rsdriver.service` restart; DimOS does not +need to restart. Process supervision cannot make a disconnected or unpowered sensor produce data. The contract is therefore: restart crashed vendor processes, detect an diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt index d001ebeb0a..65dd18ec97 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt @@ -8,10 +8,13 @@ set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +find_package(ament_cmake REQUIRED) find_package(Eigen3 REQUIRED) find_package(OpenMP QUIET) find_package(PCL 1.8 REQUIRED COMPONENTS common filters) find_package(PkgConfig REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) include(FetchContent) @@ -41,6 +44,35 @@ if(NOT DEFINED POINTLIO_DIR) set(POINTLIO_DIR ${pointlio_SOURCE_DIR}) endif() +set(POINTLIO_SOURCE_DIR ${POINTLIO_DIR}) +file(READ "${POINTLIO_SOURCE_DIR}/src/laserMapping.hpp" pointlio_mapping_source) +file(READ "${POINTLIO_SOURCE_DIR}/include/ivox/ivox3d.h" pointlio_ivox_source) +string(FIND "${pointlio_mapping_source}" "crossmat_list.reserve" pointlio_has_reserve) +string(FIND "${pointlio_mapping_source}" "crossmat_list.resize" pointlio_has_resize) +string(FIND "${pointlio_ivox_source}" "#include " pointlio_has_glog) + +if(NOT pointlio_has_reserve EQUAL -1 AND NOT pointlio_has_glog EQUAL -1) + set(POINTLIO_PATCHED_DIR "${CMAKE_BINARY_DIR}/pointlio-patched") + file(REMOVE_RECURSE "${POINTLIO_PATCHED_DIR}") + file(MAKE_DIRECTORY "${POINTLIO_PATCHED_DIR}") + file(COPY "${POINTLIO_SOURCE_DIR}/" DESTINATION "${POINTLIO_PATCHED_DIR}") + find_package(Git REQUIRED) + execute_process( + COMMAND "${GIT_EXECUTABLE}" apply "${CMAKE_CURRENT_SOURCE_DIR}/pointlio-gos.patch" + WORKING_DIRECTORY "${POINTLIO_PATCHED_DIR}" + RESULT_VARIABLE pointlio_patch_result + ERROR_VARIABLE pointlio_patch_error + ) + if(NOT pointlio_patch_result EQUAL 0) + message(FATAL_ERROR "Could not patch the pinned Point-LIO source: ${pointlio_patch_error}") + endif() + set(POINTLIO_DIR "${POINTLIO_PATCHED_DIR}") +elseif(NOT pointlio_has_resize EQUAL -1 AND pointlio_has_glog EQUAL -1) + message(STATUS "Using an already-patched Point-LIO source: ${POINTLIO_SOURCE_DIR}") +else() + message(FATAL_ERROR "Point-LIO source is neither the pinned original nor the expected patched tree") +endif() + if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) set(DIMOS_NATIVE_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../native/cpp) endif() @@ -53,7 +85,6 @@ add_executable(m20_pointlio ${POINTLIO_DIR}/src/parameters.cpp ) target_include_directories(m20_pointlio PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/compat ${POINTLIO_DIR}/include ${POINTLIO_DIR}/include/IKFoM/IKFoM_toolkit ${POINTLIO_DIR}/src @@ -66,12 +97,16 @@ target_compile_definitions(m20_pointlio PRIVATE ROOT_DIR="/tmp/m20_pointlio_" ) target_compile_options(m20_pointlio PRIVATE -Wall -Wextra -Wpedantic) -target_link_libraries(m20_pointlio PRIVATE +target_link_libraries(m20_pointlio dimos_native Eigen3::Eigen PkgConfig::LCM ${PCL_LIBRARIES} ) +ament_target_dependencies(m20_pointlio + rclcpp + sensor_msgs +) if(OpenMP_CXX_FOUND) - target_link_libraries(m20_pointlio PRIVATE OpenMP::OpenMP_CXX) + target_link_libraries(m20_pointlio OpenMP::OpenMP_CXX) endif() diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh b/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh index 679d9c4d0a..b90c5525b9 100755 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh @@ -5,6 +5,22 @@ set -euo pipefail pointlio_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ros_setup="${M20_ROS_SETUP:-/opt/robot/scripts/setup_ros2.sh}" + +if [[ -f "$ros_setup" ]]; then + # shellcheck disable=SC1090 + set +u + source "$ros_setup" + set -u +elif [[ -f /opt/ros/foxy/setup.bash ]]; then + # shellcheck disable=SC1091 + set +u + source /opt/ros/foxy/setup.bash + set -u +else + echo "M20 ROS setup not found: $ros_setup" >&2 + exit 1 +fi cmake_args=( -S "$pointlio_dir" diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h b/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h deleted file mode 100644 index e4dd8dc1af..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/compat/glog/logging.h +++ /dev/null @@ -1,8 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// The pinned Point-LIO iVox header includes glog, but all LOG calls in that -// header are commented out. Keep the M20 build free of an unused system glog -// dependency while preserving the upstream source unchanged. - -#pragma once diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp index f7a3bc6c07..aa44d36de4 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp @@ -1,10 +1,9 @@ // Copyright 2026 Dimensional Inc. // SPDX-License-Identifier: Apache-2.0 // -// M20 Point-LIO adapter. The hardware bridge publishes the robot's public -// merged RoboSense PointCloud2 and base-aligned IMU onto local DimOS LCM -// streams. This process converts those typed streams into the existing DimOS -// Point-LIO core and owns odom -> base_link. It has no vendor odometry input. +// M20 Point-LIO adapter. This process subscribes directly to the robot's public +// merged RoboSense PointCloud2 and base-aligned IMU over ROS 2, converts them +// into the pinned Point-LIO core, and owns odom -> base_link. #include #include @@ -22,6 +21,11 @@ #include #include +#include +#include +#include +#include + #include "dimos/native.hpp" #include "geometry_msgs/PoseStamped.hpp" @@ -78,9 +82,9 @@ int ivox_nearby_code(const std::string& name) { name + "'"); } -double header_seconds(const std_msgs::Header& header) { +double header_seconds(const std_msgs::msg::Header& header) { return static_cast(header.stamp.sec) + - static_cast(header.stamp.nsec) / 1e9; + static_cast(header.stamp.nanosec) / 1e9; } template @@ -99,14 +103,14 @@ struct M20FieldOffsets { std::size_t timestamp; }; -M20FieldOffsets validate_m20_cloud(const sensor_msgs::PointCloud2& cloud) { - if (cloud.height <= 0 || cloud.width <= 0) { +M20FieldOffsets validate_m20_cloud(const sensor_msgs::msg::PointCloud2& cloud) { + if (cloud.height == 0 || cloud.width == 0) { throw std::runtime_error("M20 point cloud is empty"); } - if (cloud.is_bigendian != 0) { + if (cloud.is_bigendian) { throw std::runtime_error("M20 Point-LIO requires a little-endian cloud"); } - if (cloud.point_step <= 0 || cloud.row_step <= 0) { + if (cloud.point_step == 0 || cloud.row_step == 0) { throw std::runtime_error("M20 point cloud has an invalid stride"); } const auto point_count = static_cast(cloud.width) * @@ -123,25 +127,26 @@ M20FieldOffsets validate_m20_cloud(const sensor_msgs::PointCloud2& cloud) { M20FieldOffsets offsets{missing, missing, missing, missing, missing, missing}; for (const auto& field : cloud.fields) { const auto offset = static_cast(field.offset); - if (field.count <= 0 || offset >= static_cast(cloud.point_step)) { + if (field.count == 0 || offset >= static_cast(cloud.point_step)) { continue; } - if (field.name == "x" && field.datatype == sensor_msgs::PointField::FLOAT32) { + if (field.name == "x" && + field.datatype == sensor_msgs::msg::PointField::FLOAT32) { offsets.x = offset; } else if (field.name == "y" && - field.datatype == sensor_msgs::PointField::FLOAT32) { + field.datatype == sensor_msgs::msg::PointField::FLOAT32) { offsets.y = offset; } else if (field.name == "z" && - field.datatype == sensor_msgs::PointField::FLOAT32) { + field.datatype == sensor_msgs::msg::PointField::FLOAT32) { offsets.z = offset; } else if (field.name == "intensity" && - field.datatype == sensor_msgs::PointField::FLOAT32) { + field.datatype == sensor_msgs::msg::PointField::FLOAT32) { offsets.intensity = offset; } else if (field.name == "ring" && - field.datatype == sensor_msgs::PointField::UINT16) { + field.datatype == sensor_msgs::msg::PointField::UINT16) { offsets.ring = offset; } else if (field.name == "timestamp" && - field.datatype == sensor_msgs::PointField::FLOAT64) { + field.datatype == sensor_msgs::msg::PointField::FLOAT64) { offsets.timestamp = offset; } } @@ -168,6 +173,9 @@ struct TimedPoint { } // namespace struct M20PointLioConfig { + std::string lidar_topic; + std::string imu_topic; + std::string node_name; std::string world_frame; std::string base_frame; double processing_rate_hz; @@ -231,6 +239,9 @@ struct M20PointLioConfig { bool debug; void validate() const { + require_nonempty(lidar_topic, "lidar_topic"); + require_nonempty(imu_topic, "imu_topic"); + require_nonempty(node_name, "node_name"); require_nonempty(world_frame, "world_frame"); require_nonempty(base_frame, "base_frame"); dimos::native::require_positive(processing_rate_hz, "processing_rate_hz"); @@ -268,6 +279,9 @@ struct M20PointLioConfig { // while retaining strict unknown/missing-key validation. M20PointLioConfig parse_m20_pointlio_config(Config& config) { M20PointLioConfig result{}; + result.lidar_topic = config.take("lidar_topic"); + result.imu_topic = config.take("imu_topic"); + result.node_name = config.take("node_name"); result.world_frame = config.take("world_frame"); result.base_frame = config.take("base_frame"); result.processing_rate_hz = config.take("processing_rate_hz"); @@ -340,8 +354,7 @@ class M20PointLio : public Module { void build(Builder& builder, Config& config) override { cfg_ = parse_m20_pointlio_config(config); - builder.input("raw_lidar", &M20PointLio::on_lidar, this); - builder.input("imu", &M20PointLio::on_imu, this); + lidar_ready_ = builder.output("lidar_ready"); localization_ready_ = builder.output("localization_ready"); lidar_ = builder.output("lidar"); odom_ = builder.output("odom"); @@ -418,42 +431,99 @@ class M20PointLio : public Module { params.odom_only = cfg_.odom_only; point_lio_ = std::make_unique(params, cfg_.msr_freq, cfg_.main_freq); + + rclcpp::init(0, nullptr); + dimos::native::install_signal_handlers(); + node_ = std::make_shared(cfg_.node_name); + lidar_callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + imu_callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + rclcpp::SubscriptionOptions lidar_options; + lidar_options.callback_group = lidar_callback_group_; + rclcpp::SubscriptionOptions imu_options; + imu_options.callback_group = imu_callback_group_; + const auto lidar_qos = + rclcpp::QoS(rclcpp::KeepLast(2)).reliable().durability_volatile(); + const auto imu_qos = + rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(); + lidar_subscription_ = node_->create_subscription( + cfg_.lidar_topic, lidar_qos, + [this](sensor_msgs::msg::PointCloud2::SharedPtr message) { + on_lidar(*message); + }, + lidar_options); + imu_subscription_ = node_->create_subscription( + cfg_.imu_topic, imu_qos, + [this](sensor_msgs::msg::Imu::SharedPtr message) { on_imu(*message); }, + imu_options); + executor_ = std::make_shared( + rclcpp::ExecutorOptions(), 2); + executor_->add_node(node_); + const auto now = Clock::now(); last_pointcloud_publish_ = now; last_odometry_publish_ = now; last_readiness_publish_ = now - readiness_period_; processing_thread_ = std::thread([this]() { processing_loop(); }); + spin_thread_ = std::thread([this]() { executor_->spin(); }); logging::info("M20 Point-LIO started", {logging::Field("world_frame", cfg_.world_frame), logging::Field("base_frame", cfg_.base_frame), - logging::Field("scan_lines", static_cast(cfg_.scan_line))}); + logging::Field("scan_lines", static_cast(cfg_.scan_line)), + logging::Field("lidar_topic", cfg_.lidar_topic), + logging::Field("imu_topic", cfg_.imu_topic)}); } void teardown() override { stopping_.store(true, std::memory_order_release); + if (executor_ != nullptr) { + executor_->cancel(); + } + if (spin_thread_.joinable()) { + spin_thread_.join(); + } + lidar_subscription_.reset(); + imu_subscription_.reset(); + lidar_callback_group_.reset(); + imu_callback_group_.reset(); + executor_.reset(); + node_.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } if (processing_thread_.joinable()) { processing_thread_.join(); } std_msgs::Bool ready; ready.data = 0; + lidar_ready_.publish(ready); localization_ready_.publish(ready); point_lio_.reset(); } private: - void on_lidar(const sensor_msgs::PointCloud2& source) { + void on_lidar(const sensor_msgs::msg::PointCloud2& source) { if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; const auto received_at = Clock::now(); bool feed_reserved = false; - std::unique_lock callback_lock(lidar_callback_mutex_, std::try_to_lock); - if (!callback_lock.owns_lock() || point_lio_is_processing_lidar()) { - log_busy_lidar_drop(); - return; - } - try { const auto offsets = validate_m20_cloud(source); + { + std::lock_guard lock(health_mutex_); + last_lidar_received_at_ = received_at; + have_lidar_ = true; + } + + std::unique_lock callback_lock(lidar_callback_mutex_, + std::try_to_lock); + if (!callback_lock.owns_lock() || point_lio_is_processing_lidar()) { + log_busy_lidar_drop(); + return; + } + const auto point_count = static_cast(source.width) * static_cast(source.height); const auto point_step = static_cast(source.point_step); @@ -562,13 +632,6 @@ class M20PointLio : public Module { } message->point_num = static_cast(message->points.size()); - // Sensor liveness is an ingress property. Record it as soon as the - // sample is validated, independently of estimator throughput. - { - std::lock_guard lock(health_mutex_); - last_lidar_received_at_ = received_at; - have_lidar_ = true; - } // Point-LIO's feeder callbacks take its internal buffer mutex and // are designed to run concurrently with process(). An outer lock // here would block sensor ingestion for the full estimator step. @@ -629,7 +692,7 @@ class M20PointLio : public Module { } } - void on_imu(const sensor_msgs::Imu& source) { + void on_imu(const sensor_msgs::msg::Imu& source) { if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; const auto received_at = Clock::now(); const double timestamp = header_seconds(source.header); @@ -683,6 +746,13 @@ class M20PointLio : public Module { last_imu_sensor_time_ = timestamp; } + bool lidar_health_is_fresh(Clock::time_point now) const { + std::lock_guard lock(health_mutex_); + return have_lidar_ && + now - last_lidar_received_at_ <= + std::chrono::duration(cfg_.lidar_timeout_s); + } + bool localization_health_is_fresh(Clock::time_point now) const { std::lock_guard lock(health_mutex_); return have_lidar_ && have_imu_ && have_estimate_ && @@ -754,16 +824,31 @@ class M20PointLio : public Module { } void publish_readiness(Clock::time_point now) { - const bool ready = localization_health_is_fresh(now); - std_msgs::Bool message; - message.data = static_cast(ready); - localization_ready_.publish(message); + const bool lidar_ready = lidar_health_is_fresh(now); + const bool localization_ready = localization_health_is_fresh(now); + std_msgs::Bool lidar_message; + lidar_message.data = static_cast(lidar_ready); + lidar_ready_.publish(lidar_message); + std_msgs::Bool localization_message; + localization_message.data = static_cast(localization_ready); + localization_ready_.publish(localization_message); + + const int8_t current_lidar = lidar_ready ? 1 : 0; + const int8_t previous_lidar = + lidar_readiness_state_.exchange(current_lidar, std::memory_order_acq_rel); + if (current_lidar != previous_lidar) { + if (lidar_ready) { + logging::info("M20 Point-LIO lidar input is ready"); + } else { + logging::warn("M20 Point-LIO lidar input is not ready"); + } + } - const int8_t current = ready ? 1 : 0; + const int8_t current = localization_ready ? 1 : 0; const int8_t previous = readiness_state_.exchange(current, std::memory_order_acq_rel); if (current != previous) { - if (ready) { + if (localization_ready) { logging::info("M20 Point-LIO localization is ready"); } else { logging::warn("M20 Point-LIO localization is not ready"); @@ -827,12 +912,21 @@ class M20PointLio : public Module { } M20PointLioConfig cfg_; + Output lidar_ready_; Output localization_ready_; Output lidar_; Output odom_; Output odometry_; Output tf_; std::unique_ptr point_lio_; + + std::shared_ptr node_; + std::shared_ptr executor_; + rclcpp::CallbackGroup::SharedPtr lidar_callback_group_; + rclcpp::CallbackGroup::SharedPtr imu_callback_group_; + rclcpp::Subscription::SharedPtr lidar_subscription_; + rclcpp::Subscription::SharedPtr imu_subscription_; + std::thread spin_thread_; std::thread processing_thread_; Clock::duration process_period_{}; @@ -864,6 +958,7 @@ class M20PointLio : public Module { std::atomic busy_lidar_drops_{0}; std::atomic logged_cloud_contract_{false}; std::atomic logged_cloud_sampling_{false}; + std::atomic lidar_readiness_state_{-1}; std::atomic readiness_state_{-1}; }; diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch b/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch new file mode 100644 index 0000000000..6309fb0a4e --- /dev/null +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch @@ -0,0 +1,16 @@ +Point-LIO fixes required by the M20 GOS build. + +--- a/src/laserMapping.hpp ++++ b/src/laserMapping.hpp +@@ -644,4 +644,4 @@ + /*** iterated state estimation ***/ +- crossmat_list.reserve(feats_down_size); +- pbody_list.reserve(feats_down_size); ++ crossmat_list.resize(feats_down_size); ++ pbody_list.resize(feats_down_size); + // pbody_ext_list.reserve(feats_down_size); +--- a/include/ivox/ivox3d.h ++++ b/include/ivox/ivox3d.h +@@ -8,2 +8 @@ +-#include + // #include diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py index 4e7fb09da1..aa8c14b1ce 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/module.py +++ b/dimos/robot/deeprobotics/m20/pointlio/module.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Native Point-LIO wrapper for the M20's typed raw LiDAR and IMU streams.""" +"""Native Point-LIO wrapper with direct M20 ROS lidar and IMU ingress.""" from __future__ import annotations @@ -21,10 +21,9 @@ from pydantic import Field from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.core.stream import In, Out +from dimos.core.stream import Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.sensor_msgs.Imu import Imu from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.tf2_msgs.TFMessage import TFMessage @@ -40,10 +39,19 @@ class M20PointLioConfig(NativeModuleConfig): executable: str = "build/m20_pointlio" build_command: str | None = "./build.sh" stdin_config: bool = True + extra_env: dict[str, str] = Field( + default_factory=lambda: { + "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", + "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", + } + ) # GOS isolates its RK3588 big cores. Cores 6-7 run the vendor lidar # drivers, so Point-LIO owns the otherwise-idle big cores 4-5. cpu_affinity: frozenset[int] | None = frozenset({4, 5}) + lidar_topic: str = "/LIDAR/POINTS" + imu_topic: str = "/IMU" + node_name: str = "dimos_m20_pointlio" world_frame: str = "odom" base_frame: str = "base_link" processing_rate_hz: float = Field(default=1000.0, gt=0.0) @@ -123,18 +131,16 @@ class M20PointLioConfig(NativeModuleConfig): class M20PointLio(NativeModule, perception.Lidar, perception.Odometry): - """Run the existing DimOS Point-LIO core on M20 raw sensor streams. + """Run the pinned Point-LIO core directly on the M20's ROS sensor topics. - The module deliberately has no vendor odometry input. It consumes only the - merged ``base_link`` cloud and 200 Hz ``base_link`` IMU produced by the M20 - hardware bridge, then owns the ``odom -> base_link`` transform. + The native process subscribes to the merged ``base_link`` cloud and 200 Hz + ``base_link`` IMU itself, avoiding a full-payload LCM hop through the command + bridge. It has no vendor odometry input and owns ``odom -> base_link``. """ config: M20PointLioConfig - raw_lidar: In[PointCloud2] - imu: In[Imu] - + lidar_ready: Out[Bool] localization_ready: Out[Bool] lidar: Out[PointCloud2] odom: Out[PoseStamped] From 701e415dff028551e35fe2855915d6efcd2751ae Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 18:03:26 +0800 Subject: [PATCH 05/15] refactor(robot): slim M20 integration --- dimos/mapping/ray_tracing/rust/src/module.rs | 48 --- dimos/robot/all_blueprints.py | 2 - dimos/robot/deeprobotics/__init__.py | 15 - dimos/robot/deeprobotics/m20/__init__.py | 15 - .../deeprobotics/m20/blueprints/__init__.py | 15 - .../m20/blueprints/m20_kronknav.py | 143 +++---- .../m20/blueprints/test_m20_kronknav.py | 165 ------- dimos/robot/deeprobotics/m20/bridge/README.md | 34 -- .../robot/deeprobotics/m20/bridge/__init__.py | 19 - dimos/robot/deeprobotics/m20/bridge/module.py | 1 + dimos/robot/deeprobotics/m20/connection.py | 42 +- dimos/robot/deeprobotics/m20/deploy/README.md | 198 ++------- .../deeprobotics/m20/deploy/dimos-m20.env | 3 - .../deeprobotics/m20/pointlio/__init__.py | 15 - .../deeprobotics/m20/pointlio/cpp/main.cpp | 158 +------ .../robot/deeprobotics/m20/pointlio/module.py | 12 +- .../deeprobotics/m20/pointlio/test_module.py | 42 -- .../robot/deeprobotics/m20/test_connection.py | 401 ------------------ native/cpp/tests/test_config.cpp | 21 - 19 files changed, 125 insertions(+), 1224 deletions(-) delete mode 100644 dimos/robot/deeprobotics/__init__.py delete mode 100644 dimos/robot/deeprobotics/m20/__init__.py delete mode 100644 dimos/robot/deeprobotics/m20/blueprints/__init__.py delete mode 100644 dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py delete mode 100644 dimos/robot/deeprobotics/m20/bridge/README.md delete mode 100644 dimos/robot/deeprobotics/m20/bridge/__init__.py delete mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20.env delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/__init__.py delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/test_module.py delete mode 100644 dimos/robot/deeprobotics/m20/test_connection.py diff --git a/dimos/mapping/ray_tracing/rust/src/module.rs b/dimos/mapping/ray_tracing/rust/src/module.rs index ca601944a1..674bc2ae9f 100644 --- a/dimos/mapping/ray_tracing/rust/src/module.rs +++ b/dimos/mapping/ray_tracing/rust/src/module.rs @@ -411,54 +411,6 @@ mod tests { ) } - #[test] - fn extract_xyz_accepts_the_m20_extended_point_layout() { - let field = |name: &str, offset: i32, datatype: u8| PointField { - name: name.into(), - offset, - datatype, - count: 1, - }; - let expected = [(1.25_f32, -2.5_f32, 0.75_f32), (-4.0_f32, 5.5_f32, 1.0_f32)]; - let mut data = vec![0_u8; expected.len() * 26]; - for (index, &(x, y, z)) in expected.iter().enumerate() { - let base = index * 26; - data[base..base + 4].copy_from_slice(&x.to_le_bytes()); - data[base + 4..base + 8].copy_from_slice(&y.to_le_bytes()); - data[base + 8..base + 12].copy_from_slice(&z.to_le_bytes()); - data[base + 12..base + 16].copy_from_slice(&42.0_f32.to_le_bytes()); - data[base + 16..base + 18].copy_from_slice(&7_u16.to_le_bytes()); - data[base + 18..base + 26].copy_from_slice(&1_718_663_385.5_f64.to_le_bytes()); - } - let cloud = PointCloud2 { - header: Header { - frame_id: "base_link".into(), - ..Header::default() - }, - height: 1, - width: expected.len() as i32, - fields: vec![ - field("x", 0, PointField::FLOAT32 as u8), - field("y", 4, PointField::FLOAT32 as u8), - field("z", 8, PointField::FLOAT32 as u8), - field("intensity", 12, PointField::FLOAT32 as u8), - field("ring", 16, PointField::UINT16 as u8), - field("timestamp", 18, PointField::FLOAT64 as u8), - ], - is_bigendian: false, - point_step: 26, - row_step: expected.len() as i32 * 26, - data, - is_dense: false, - }; - - let Ok(decoded) = extract_xyz(&cloud) else { - panic!("M20 clouds must be mapper-compatible"); - }; - - assert_eq!(decoded, expected); - } - /// The clear-mask handler names voxels by decoding a cloud and quantizing /// it. Both halves have to agree with how returns were quantized on the way /// in, or a mask silently clears nothing. diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index a99b6acea1..14ac1e5bec 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -42,9 +42,7 @@ "coordinator-velocity-xarm6": "dimos.robot.manipulators.xarm.blueprints.teleop:coordinator_velocity_xarm6", "coordinator-xarm6": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm6", "coordinator-xarm7": "dimos.robot.manipulators.xarm.blueprints.basic:coordinator_xarm7", - "deeprobotics-m20-kronknav": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav", "deeprobotics-m20-kronknav-control": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_kronknav_control", - "deeprobotics-m20-pointlio": "dimos.robot.deeprobotics.m20.blueprints.m20_kronknav:deeprobotics_m20_pointlio", "demo-agent": "dimos.agents.demo_agent:demo_agent", "demo-agent-camera": "dimos.agents.demo_agent:demo_agent_camera", "demo-camera": "dimos.hardware.sensors.camera.module:demo_camera", diff --git a/dimos/robot/deeprobotics/__init__.py b/dimos/robot/deeprobotics/__init__.py deleted file mode 100644 index 6cf15a5bc4..0000000000 --- a/dimos/robot/deeprobotics/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Deep Robotics integrations.""" diff --git a/dimos/robot/deeprobotics/m20/__init__.py b/dimos/robot/deeprobotics/m20/__init__.py deleted file mode 100644 index 624498841a..0000000000 --- a/dimos/robot/deeprobotics/m20/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Deep Robotics Lynx M20 integration.""" diff --git a/dimos/robot/deeprobotics/m20/blueprints/__init__.py b/dimos/robot/deeprobotics/m20/blueprints/__init__.py deleted file mode 100644 index 4e1ae872dc..0000000000 --- a/dimos/robot/deeprobotics/m20/blueprints/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Runnable M20 blueprints.""" diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 0537e46bdd..81aa16f831 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -16,7 +16,7 @@ from typing import Any -from dimos.core.coordination.blueprints import Blueprint, autoconnect +from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC @@ -114,89 +114,62 @@ def _m20_rerun_blueprint() -> Any: } -# Safe hardware bring-up graph: direct ROS sensors -> native Point-LIO -> Rerun. -# It intentionally has no connection/controller modules and cannot publish -# /NAV_CMD. Use this before starting the full mapper/planner blueprint. -deeprobotics_m20_pointlio = autoconnect( +deeprobotics_m20_kronknav_control = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + M20ROSBridge.blueprint( + enable_command_output=True, + max_linear_x=MAX_LINEAR_X_M_S, + max_linear_y=MAX_LINEAR_Y_M_S, + max_angular_z=MAX_ANGULAR_Z_RAD_S, + ), M20PointLio.blueprint(), -).global_config(n_workers=2, transport="lcm") - - -def _m20_kronknav(*, enable_command_output: bool) -> Blueprint: - """Compose one complete M20 graph on GOS. - - The boolean is intentionally fixed by the two exported blueprints below; - selecting the control blueprint is the deployment-time ownership decision. - Both still start with the operator command arm disarmed. - """ - return autoconnect( - vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), - M20ROSBridge.blueprint( - enable_command_output=enable_command_output, - max_linear_x=MAX_LINEAR_X_M_S, - max_linear_y=MAX_LINEAR_Y_M_S, - max_angular_z=MAX_ANGULAR_Z_RAD_S, - ), - M20PointLio.blueprint(), - M20Connection.blueprint( - max_linear_x=MAX_LINEAR_X_M_S, - max_linear_y=MAX_LINEAR_Y_M_S, - max_angular_z=MAX_ANGULAR_Z_RAD_S, - ), - RayTracingVoxelMap.blueprint( - voxel_size=VOXEL_SIZE_M, - max_range=25.0, - emit_every=1, - global_emit_every=50, - support_min=4, - world_frame="odom", - worker_threads=3, - ), - MLSPlannerNative.blueprint( - world_frame="odom", - base_frame="base_link", - voxel_size=VOXEL_SIZE_M, - robot_height=PLANNING_HEIGHT_M, - start_z_offset_m=BASE_LINK_HEIGHT_M, - wall_clearance_m=0.3, - wall_buffer_m=0.85, - wall_buffer_weight=100.0, - step_threshold_m=0.12, - step_penalty_weight=4.0, - viz_publish_hz=PLANNER_VIZ_HZ, - worker_threads=2, - ).remappings( - [ - (MLSPlannerNative, "global_map", "global_map_unused"), - (MLSPlannerNative, "path", "planner_path"), - ] - ), - DanLocalPlanner.blueprint( - lock_replan=0.4, - resample_spacing_m=0.1, - ), - DanHolonomicTC.blueprint( - run_profile="walk", - speed_m_s=CRUISE_SPEED_M_S, - control_frequency=10.0, - ), - MovementManager.blueprint(), - ).global_config( - n_workers=4, - obstacle_avoidance=False, - robot_width=BODY_WIDTH_M, - robot_rotation_diameter=ROTATION_DIAMETER_M, - transport="lcm", - ) - - -# Safe default: mapping, planning, and Rerun are live, but the native process -# does not create a /NAV_CMD publisher and M20Connection can never become ready. -deeprobotics_m20_kronknav = autoconnect(_m20_kronknav(enable_command_output=False)) - -# Explicit control ownership: creates /NAV_CMD. A single M20Connection.standup() -# call performs the vendor state/gait sequence and arms after the robot confirms -# its RL-Control command path. Mapper health remains navigation diagnostics, as -# it does in the Go2 stack; it is not a manual-teleop latch. -deeprobotics_m20_kronknav_control = autoconnect(_m20_kronknav(enable_command_output=True)) + M20Connection.blueprint( + max_linear_x=MAX_LINEAR_X_M_S, + max_linear_y=MAX_LINEAR_Y_M_S, + max_angular_z=MAX_ANGULAR_Z_RAD_S, + ), + RayTracingVoxelMap.blueprint( + voxel_size=VOXEL_SIZE_M, + max_range=25.0, + emit_every=1, + global_emit_every=50, + support_min=4, + world_frame="odom", + worker_threads=3, + ), + MLSPlannerNative.blueprint( + world_frame="odom", + base_frame="base_link", + voxel_size=VOXEL_SIZE_M, + robot_height=PLANNING_HEIGHT_M, + start_z_offset_m=BASE_LINK_HEIGHT_M, + wall_clearance_m=0.3, + wall_buffer_m=0.85, + wall_buffer_weight=100.0, + step_threshold_m=0.12, + step_penalty_weight=4.0, + viz_publish_hz=PLANNER_VIZ_HZ, + worker_threads=2, + ).remappings( + [ + (MLSPlannerNative, "global_map", "global_map_unused"), + (MLSPlannerNative, "path", "planner_path"), + ] + ), + DanLocalPlanner.blueprint( + lock_replan=0.4, + resample_spacing_m=0.1, + ), + DanHolonomicTC.blueprint( + run_profile="walk", + speed_m_s=CRUISE_SPEED_M_S, + control_frequency=10.0, + ), + MovementManager.blueprint(), +).global_config( + n_workers=4, + obstacle_avoidance=False, + robot_width=BODY_WIDTH_M, + robot_rotation_diameter=ROTATION_DIAMETER_M, + transport="lcm", +) diff --git a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py deleted file mode 100644 index e01f9e6c16..0000000000 --- a/dimos/robot/deeprobotics/m20/blueprints/test_m20_kronknav.py +++ /dev/null @@ -1,165 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Safety contract tests for the exported M20 blueprints.""" - -from dimos.core.coordination.blueprints import Blueprint -from dimos.mapping.ray_tracing.module import RayTracingVoxelMap -from dimos.msgs.geometry_msgs.PointStamped import PointStamped -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.nav_msgs.Path import Path -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Bool import Bool -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC -from dimos.navigation.dannav.local_planner.module import DanLocalPlanner -from dimos.navigation.movement_manager.movement_manager import MovementManager -from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative -from dimos.robot.deeprobotics.m20.blueprints.m20_kronknav import ( - deeprobotics_m20_kronknav, - deeprobotics_m20_kronknav_control, - deeprobotics_m20_pointlio, -) -from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge -from dimos.robot.deeprobotics.m20.connection import M20Connection -from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLio -from dimos.visualization.rerun.bridge import RerunBridgeModule -from dimos.visualization.rerun.websocket_server import RerunWebSocketServer -from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule - - -def _bridge_kwargs(blueprint: Blueprint) -> dict[str, object]: - atoms = [atom for atom in blueprint.blueprints if atom.module is M20ROSBridge] - assert len(atoms) == 1 - return atoms[0].kwargs - - -def _module_kwargs(blueprint: Blueprint, module: type) -> dict[str, object]: - atoms = [atom for atom in blueprint.active_blueprints if atom.module is module] - assert len(atoms) == 1 - return atoms[0].kwargs - - -def _endpoint_modules( - blueprint: Blueprint, - name: str, - stream_type: type, - direction: str, -) -> set[type]: - result: set[type] = set() - for atom in blueprint.active_blueprints: - for stream in atom.streams: - effective_name = blueprint.remapping_map.get((atom.name, stream.name), stream.name) - if ( - effective_name == name - and stream.type is stream_type - and stream.direction == direction - ): - result.add(atom.module) - return result - - -def test_default_kronknav_blueprint_cannot_publish_robot_commands() -> None: - assert _bridge_kwargs(deeprobotics_m20_kronknav)["enable_command_output"] is False - - -def test_pointlio_bringup_blueprint_has_no_command_publisher() -> None: - assert not any(atom.module is M20ROSBridge for atom in deeprobotics_m20_pointlio.blueprints) - assert not any(atom.module is M20Connection for atom in deeprobotics_m20_pointlio.blueprints) - - -def test_control_blueprint_explicitly_enables_robot_command_publisher() -> None: - assert _bridge_kwargs(deeprobotics_m20_kronknav_control)["enable_command_output"] is True - - -def test_m20_blueprints_pin_native_sdk_supported_local_transport() -> None: - assert deeprobotics_m20_kronknav.global_config_overrides["transport"] == "lcm" - assert deeprobotics_m20_kronknav_control.global_config_overrides["transport"] == "lcm" - - -def test_sensor_and_pose_streams_reach_mapping_and_navigation() -> None: - blueprint = deeprobotics_m20_kronknav - - assert not _endpoint_modules(blueprint, "raw_lidar", PointCloud2, "out") - assert _endpoint_modules(blueprint, "lidar", PointCloud2, "out") == {M20PointLio} - assert RayTracingVoxelMap in _endpoint_modules(blueprint, "lidar", PointCloud2, "in") - assert _endpoint_modules(blueprint, "lidar_ready", Bool, "out") == {M20PointLio} - assert _endpoint_modules(blueprint, "lidar_ready", Bool, "in") == {M20Connection} - assert _endpoint_modules(blueprint, "localization_ready", Bool, "out") == {M20PointLio} - assert _endpoint_modules(blueprint, "localization_ready", Bool, "in") == {M20Connection} - assert _endpoint_modules(blueprint, "tf", TFMessage, "out") == {M20PointLio} - assert _endpoint_modules(blueprint, "tf", TFMessage, "in") == { - RayTracingVoxelMap, - MLSPlannerNative, - } - assert _endpoint_modules(blueprint, "odom", PoseStamped, "out") == {M20PointLio} - assert { - DanLocalPlanner, - DanHolonomicTC, - } <= _endpoint_modules(blueprint, "odom", PoseStamped, "in") - - -def test_kronknav_path_and_guarded_command_chain_is_complete() -> None: - blueprint = deeprobotics_m20_kronknav_control - - assert _endpoint_modules(blueprint, "planner_path", Path, "out") == {MLSPlannerNative} - assert _endpoint_modules(blueprint, "planner_path", Path, "in") == {DanLocalPlanner} - assert _endpoint_modules(blueprint, "path", Path, "out") == {DanLocalPlanner} - assert DanHolonomicTC in _endpoint_modules(blueprint, "path", Path, "in") - assert _endpoint_modules(blueprint, "cmd_vel", Twist, "out") == {MovementManager} - assert _endpoint_modules(blueprint, "cmd_vel", Twist, "in") == {M20Connection} - assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "out") == {M20Connection} - assert _endpoint_modules(blueprint, "safe_cmd_vel", Twist, "in") == {M20ROSBridge} - - -def test_rerun_click_and_teleop_inputs_reach_navigation_and_control() -> None: - blueprint = deeprobotics_m20_kronknav_control - - assert _endpoint_modules(blueprint, "clicked_point", PointStamped, "out") == { - RerunWebSocketServer - } - assert _endpoint_modules(blueprint, "clicked_point", PointStamped, "in") == {MovementManager} - assert _endpoint_modules(blueprint, "tele_cmd_vel", Twist, "out") == { - RerunWebSocketServer, - WebsocketVisModule, - } - assert _endpoint_modules(blueprint, "tele_cmd_vel", Twist, "in") == {MovementManager} - assert _endpoint_modules(blueprint, "goal", PointStamped, "out") == {MovementManager} - assert _endpoint_modules(blueprint, "goal", PointStamped, "in") == { - MLSPlannerNative, - DanLocalPlanner, - } - assert _endpoint_modules(blueprint, "nav_cmd_vel", Twist, "out") == {DanHolonomicTC} - assert _endpoint_modules(blueprint, "nav_cmd_vel", Twist, "in") == {MovementManager} - - -def test_rerun_uses_go2_navigation_data_budget() -> None: - config = _module_kwargs(deeprobotics_m20_kronknav_control, RerunBridgeModule) - visual_override = config["visual_override"] - max_hz = config["max_hz"] - - assert isinstance(visual_override, dict) - assert "world/raw_lidar" not in visual_override - assert "world/imu" not in visual_override - assert visual_override["world/lidar"] is None - assert isinstance(max_hz, dict) - assert max_hz["world/local_map"] == 0.5 - assert config["memory_limit"] == "64MB" - - -def test_global_map_is_rate_limited_at_the_source() -> None: - config = _module_kwargs(deeprobotics_m20_kronknav_control, RayTracingVoxelMap) - - assert config["global_emit_every"] == 50 diff --git a/dimos/robot/deeprobotics/m20/bridge/README.md b/dimos/robot/deeprobotics/m20/bridge/README.md deleted file mode 100644 index 51a009d2df..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/README.md +++ /dev/null @@ -1,34 +0,0 @@ -# M20 command/state ROS bridge - -This native module runs on the M20 Pro GOS computer and links against the -installed ROS 2 Foxy and `drdds` packages. It carries only low-bandwidth robot -command and state traffic between ROS and typed DimOS streams. - -Lidar and IMU do not pass through this bridge. `M20PointLio` subscribes directly -to `/LIDAR/POINTS` and `/IMU` in its own native process, validates the X20 cloud -contract, converts the IMU units, and publishes `lidar_ready`, localization, -odometry, TF, and the map-ready cloud. Keeping the sensor payload out of this -bridge avoids copying and serializing every 0.8-2.1 MB cloud through LCM. - -The bridge subscribes to the vendor motion and hard-estop state topics. When -`enable_command_output` is explicitly enabled, it owns the `/NAV_CMD`, -`/MOTION_STATE`, and `/GAIT` publishers. Startup does not change the robot mode, -gait, charging state, or standing state. - -The current C++ NativeModule SDK carries DimOS command/state streams over local -LCM. The M20 blueprints therefore pin the onboard graph to LCM; this bridge does -not contain a private Zenoh implementation. - -Build on GOS after sourcing the vendor environment: - -```bash -./build.sh -``` - -The default setup path is `/opt/robot/scripts/setup_ros2.sh`; override it with -`M20_ROS_SETUP` if necessary. The build intentionally fails off-robot when -Foxy and the installed `drdds` package are unavailable. For an offline build, -set `DIMOS_LCM_DIR` to the pinned `dimos-lcm` checkout. - -See [`deploy/README.md`](../deploy/README.md) for the robot service setup, -command ownership, and launch procedure. diff --git a/dimos/robot/deeprobotics/m20/bridge/__init__.py b/dimos/robot/deeprobotics/m20/bridge/__init__.py deleted file mode 100644 index a357d1f6f8..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Robot-local ROS 2/DrDDS bridge for the Lynx M20.""" - -from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge - -__all__ = ["M20ROSBridge"] diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py index bda15cfac2..e321ebada6 100644 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -45,6 +45,7 @@ class M20ROSBridgeConfig(NativeModuleConfig): # runtime dependency explicit and reproducible. extra_env: dict[str, str] = Field( default_factory=lambda: { + "FASTRTPS_DEFAULT_PROFILES_FILE": "/opt/robot/fastdds.xml", "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", } diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index 7a4fbe6de7..9057db83ea 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -109,9 +109,7 @@ class M20Connection(Module): The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, rejects it until ``standup()`` has armed control, bounds planar commands, and - emits ``safe_cmd_vel`` for the robot-local bridge. Lidar and localization - readiness are diagnostic signals for the navigation stack; like the Go2 - connection, they do not permanently disable manual velocity control. + emits ``safe_cmd_vel`` for the robot-local bridge. ``standup()`` is the normal one-call operator entry point: it completes the vendor state and gait transitions, waits for the guarded command path, and @@ -123,8 +121,6 @@ class M20Connection(Module): cmd_vel: In[Twist] command_ready: In[Bool] - lidar_ready: In[Bool] - localization_ready: In[Bool] motion_state: In[Int32] gait_state: In[UInt32] safe_cmd_vel: Out[Twist] @@ -138,8 +134,6 @@ def __init__(self, **kwargs: Any) -> None: self._state_condition = Condition(self._lock) self._armed = False self._command_ready = False - self._lidar_ready = False - self._localization_ready = False self._motion_state: int | None = None self._gait_state: int | None = None self._basic_server_message_id = 0 @@ -148,10 +142,6 @@ def __init__(self, **kwargs: Any) -> None: def start(self) -> None: super().start() self.register_disposable(Disposable(self.command_ready.subscribe(self._on_command_ready))) - self.register_disposable(Disposable(self.lidar_ready.subscribe(self._on_lidar_ready))) - self.register_disposable( - Disposable(self.localization_ready.subscribe(self._on_localization_ready)) - ) self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) self.register_disposable(Disposable(self.motion_state.subscribe(self._on_motion_state))) self.register_disposable(Disposable(self.gait_state.subscribe(self._on_gait_state))) @@ -197,18 +187,6 @@ def is_command_ready(self) -> bool: with self._lock: return self._command_ready - @rpc - def is_lidar_ready(self) -> bool: - """Return whether the native bridge has received a valid cloud within its timeout.""" - with self._lock: - return self._lidar_ready - - @rpc - def is_localization_ready(self) -> bool: - """Return whether the native M20 Point-LIO estimator is healthy and publishing.""" - with self._lock: - return self._localization_ready - @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: """Forward a bounded planar velocity when armed. @@ -316,24 +294,6 @@ def _on_command_ready(self, msg: Bool) -> None: self.safe_cmd_vel.publish(Twist.zero()) logger.warning("M20 command output temporarily inhibited: robot control path is stale") - def _on_lidar_ready(self, msg: Bool) -> None: - ready = bool(msg.data) - with self._state_condition: - changed = self._lidar_ready != ready - self._lidar_ready = ready - self._state_condition.notify_all() - if changed and ready: - logger.info("M20 lidar stream became ready") - - def _on_localization_ready(self, msg: Bool) -> None: - ready = bool(msg.data) - with self._state_condition: - changed = self._localization_ready != ready - self._localization_ready = ready - self._state_condition.notify_all() - if changed and ready: - logger.info("M20 Point-LIO localization became ready") - def _on_motion_state(self, msg: Int32) -> None: with self._state_condition: self._motion_state = int(msg.data) diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index 3e78b795d7..c82422beae 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -1,33 +1,23 @@ -# M20 lidar deployment +# M20 deployment -DimOS consumes one merged cloud on GOS. The verified data path is: +The X20/M20 integration has one runnable blueprint: +`deeprobotics-m20-kronknav-control`. It starts command output disarmed. + +The sensor path stays on GOS and never sends raw lidar through LCM: ```text -front lidar 10.21.33.201 -- MSOP 6691 / DIFOP 7781 --+ - +--> NOS multicast-relay.service -rear lidar 10.21.33.202 -- MSOP 6692 / DIFOP 7782 --+ --> GOS rsdriver.service - --> DDS /LIDAR/POINTS - --> M20PointLio - --> RayTracingVoxelMap +M20 lidar + IMU -- Fast DDS --> M20PointLio -- LCM --> mapping/navigation +M20 state/control -- Fast DDS <--> M20ROSBridge -- LCM <--> M20Connection ``` -The two lidars are already extrinsically merged by the vendor driver. On the -inspected robot, `/LIDAR/POINTS` is reliable/volatile at about 9.5 Hz, with -29,000-81,000 points and 0.8-2.1 MB per cloud. Its 26-byte point layout is: - -| field | type | byte offset | -|---|---|---:| -| `x` | float32 | 0 | -| `y` | float32 | 4 | -| `z` | float32 | 8 | -| `intensity` | float32 | 12 | -| `ring` | uint16 | 16 | -| `timestamp` | float64 | 18 | +`/LIDAR/POINTS` is the vendor-merged `base_link` cloud. Its point records are +little-endian `float32 x/y/z/intensity`, `uint16 ring`, and `float64 timestamp` +at byte offsets 0, 4, 8, 12, 16, and 18 respectively. ## Persistent robot setup -NOS must have its existing relay enabled. Install the checked-in supervisor and -drop-in before enabling it: +On NOS (`10.21.31.106`), install the lidar relay supervisor and reserve +`/NAV_CMD` for DimOS: ```bash sudo install -D -o root -g root -m 0755 \ @@ -36,43 +26,16 @@ sudo install -D -o root -g root -m 0755 \ sudo install -D -o root -g root -m 0644 \ dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf \ /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf -sudo systemctl daemon-reload -sudo systemctl enable --now multicast-relay.service -systemctl is-enabled multicast-relay.service -systemctl is-active multicast-relay.service -``` - -Run those commands on NOS (`10.21.31.106`). - -The vendor Python process starts four forwarding threads but does not propagate -a worker-thread failure to systemd. It can therefore remain `active` while one -or both MSOP streams are dead. The supervisor waits for both NOS Ethernet -addresses and multicast routes before launch, then restarts the service if the -process has fewer than its expected four forwarding workers. - -The control blueprint owns `/NAV_CMD`. The M20 manual explicitly requires the -vendor `planner.service` to be stopped before an external publisher uses that -topic. Install the checked-in ownership condition on NOS so a boot script cannot -silently start a second command owner: - -```bash sudo install -D -o root -g root -m 0644 \ dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf \ /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf sudo systemctl unmask planner.service sudo systemctl daemon-reload sudo systemctl stop planner.service -systemctl is-active planner.service # must print inactive +sudo systemctl enable --now multicast-relay.service ``` -To deliberately restore the vendor planner, create -`/etc/dimos/enable-vendor-m20-planner` and start the service. Never run it while -the DimOS control blueprint owns `/NAV_CMD`. - -GOS runs `rsdriver.service` as root for real-time scheduling. Fast DDS therefore -creates root-owned shared-memory files that an unprivileged DimOS process cannot -attach to. From a DimOS checkout on GOS, install the checked-in permission helper -and systemd drop-in, then enable the driver: +On GOS (`10.21.31.104`), install the Fast DDS shared-memory permission hooks: ```bash sudo install -D -o root -g root -m 0755 \ @@ -81,126 +44,59 @@ sudo install -D -o root -g root -m 0755 \ sudo install -D -o root -g root -m 0644 \ dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf \ /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf +sudo install -D -o root -g root -m 0644 \ + dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path \ + /etc/systemd/system/dimos-m20-fastdds-permissions.path +sudo install -D -o root -g root -m 0644 \ + dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service \ + /etc/systemd/system/dimos-m20-fastdds-permissions.service sudo systemctl daemon-reload -sudo systemctl enable --now rsdriver.service -``` - -The drop-in is outside the vendor package, so it survives package replacement. -The vendor package's post-install script may disable `rsdriver.service`; re-run -the `enable --now` command after a driver or firmware update. - -## Cable-free operator access - -GOS has no Wi-Fi radio. Use the vendor-managed AP on AOS while DimOS continues -to run entirely on GOS. The inspected robot exposes `m20_24G`; NetworkManager -gives clients an address in `10.21.41.0/24`, with AOS at `10.21.41.1`, and -routes them to the internal `10.21.31.0/24` network. - -The AP credential is robot configuration and is intentionally not stored in -this repository. It can be read or changed on AOS using the vendor Wi-Fi tools. -The vendor `start.service` launches `loop_start_ap.sh` at boot, which -recreates the AP when `wlan0` is down. No DimOS service is required on AOS. - -If an old office-client experiment is installed, return AOS to the vendor AP: - -```bash -sudo systemctl disable --now dimos-m20-office-wifi.service 2>/dev/null || true -sudo systemctl disable --now wifi-office-autoswitch.service 2>/dev/null || true -sudo systemctl disable --now zenoh-router.service 2>/dev/null || true -sudo nmcli connection modify office5g connection.autoconnect no 2>/dev/null || true -sudo nmcli connection down office5g 2>/dev/null || true +sudo systemctl enable --now rsdriver.service dimos-m20-fastdds-permissions.path ``` -After a few seconds, verify on AOS: +The `rsdriver` hook fixes existing Fast DDS files after driver start. The path +unit handles files created later. Keep both. -```bash -iw dev wlan0 info -ip -4 address show dev wlan0 -``` +## Run on GOS -The output must show `type AP`, the intended SSID, and -`10.21.41.1/24`. Connect the developer computer to that SSID; it should use -DHCP. On macOS, add the robot-subnet route explicitly so a simultaneous USB -phone tether remains the internet default: +The native module declarations carry their own ROS library path, RMW selection, +and Fast DDS profile. No ROS setup script or environment file is required: ```bash -networksetup -setadditionalroutes \ - "Wi-Fi" 10.21.31.0 255.255.255.0 10.21.41.1 -route -n get 10.21.31.104 -ping 10.21.31.104 -ssh user@10.21.31.104 -dimos-viewer \ - --connect rerun+http://10.21.31.104:9877/proxy \ - --ws-url ws://10.21.31.104:3030/ws +cd /var/opt/robot/data/dimos-m20-kronknav +source .venv/bin/activate +LCM_DEFAULT_URL='udpm://239.255.76.67:7667?ttl=0&recv_buf_size=16777216' \ + dimos --rerun-open none --rerun-host 0.0.0.0 \ + run deeprobotics-m20-kronknav-control ``` -Use a separate USB phone tether if the developer computer also needs internet; -the robot AP is the robot route, not the office internet connection. No DimOS -module, LCM traffic, ROS/DDS traffic, or Zenoh router runs on AOS. - -## Run DimOS locally on GOS - -Load the local-only LCM URL before launching DimOS. Raw lidar and IMU remain in -Fast DDS and enter `M20PointLio` directly; they are not serialized through LCM. -Both native ROS processes need the vendor Foxy library path and Fast DDS profile -in their inherited environment: +Attach and enable control deliberately: ```bash -source /opt/ros/foxy/setup.bash -export FASTRTPS_DEFAULT_PROFILES_FILE=/opt/robot/fastdds.xml -set -a -source dimos/robot/deeprobotics/m20/deploy/dimos-m20.env -set +a -dimos --rerun-open none --rerun-host 0.0.0.0 \ - run deeprobotics-m20-kronknav +dimos --transport lcm shell ``` -After a daemon launch, use `dimos status` and `dimos log` to confirm that both -`M20ROSBridge` and `M20PointLio` remained alive. A successful viewer connection -alone proves only the visualization process, not the sensor bridge. - -Attach a viewer over direct robot Ethernet or the onboard AP without moving the -DimOS graph off GOS: - -```bash -dimos-viewer \ - --connect rerun+http://10.21.31.104:9877/proxy \ - --ws-url ws://10.21.31.104:3030/ws +```python +app.M20Connection.standup() ``` -The default blueprint never creates a `/NAV_CMD` publisher. Use the separate -`deeprobotics-m20-kronknav-control` blueprint only when motion ownership is -intentional; it still starts disarmed. Its single `M20Connection.standup()` RPC -switches `basic_server` to navigation usage mode (`Type=1101`, `Command=5`, -`Mode=1`), transitions to RL Control, selects gait `0x3002`, and arms commands. +`standup()` selects the navigation usage mode, enters RL Control, chooses gait +`0x3002`, and arms bounded velocity output. -## Health and recovery contract - -Before starting DimOS, both checks below must print `enabled` and `active` on -their respective hosts: +## Minimal checks ```bash -systemctl is-enabled multicast-relay.service # NOS +# NOS systemctl is-active multicast-relay.service +systemctl is-active planner.service # must be inactive -systemctl is-enabled rsdriver.service # GOS +# GOS systemctl is-active rsdriver.service +systemctl is-active dimos-m20-fastdds-permissions.path +dimos status +dimos log ``` -As the normal `user` account on GOS, this measures the DDS stream itself rather -than merely checking that the driver process exists: - -```bash -source /opt/robot/scripts/setup_ros2.sh -ros2 topic hz --wall-time --window 100 /LIDAR/POINTS -``` - -The native `M20PointLio` process validates every cloud and publishes -`lidar_ready` at 10 Hz. Five missed nominal frames (0.5 seconds) make it false. -DDS rematches automatically after an `rsdriver.service` restart; DimOS does not -need to restart. - -Process supervision cannot make a disconnected or unpowered sensor produce -data. The contract is therefore: restart crashed vendor processes, detect an -invalid or absent stream within 0.5 seconds, fail closed, expose the state, and -recover automatically when valid clouds return. +To restore the vendor planner instead of DimOS control, create +`/etc/dimos/enable-vendor-m20-planner` on NOS and start `planner.service`. Never +run both command owners at once. diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env b/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env deleted file mode 100644 index 8f55f85fe5..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20.env +++ /dev/null @@ -1,3 +0,0 @@ -# Local-only LCM bus with an explicit receive buffer for 0.8-2.1 MB clouds. -DIMOS_TRANSPORT=lcm -LCM_DEFAULT_URL="udpm://239.255.76.67:7667?ttl=0&recv_buf_size=16777216" diff --git a/dimos/robot/deeprobotics/m20/pointlio/__init__.py b/dimos/robot/deeprobotics/m20/pointlio/__init__.py deleted file mode 100644 index 1dc502c8bb..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""DimOS-native Point-LIO adapter for the Deep Robotics M20.""" diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp index aa44d36de4..8e6ac4bee1 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp @@ -30,11 +30,9 @@ #include "geometry_msgs/PoseStamped.hpp" #include "geometry_msgs/TransformStamped.hpp" -#include "nav_msgs/Odometry.hpp" #include "sensor_msgs/Imu.hpp" #include "sensor_msgs/PointCloud2.hpp" #include "sensor_msgs/PointField.hpp" -#include "std_msgs/Bool.hpp" #include "tf2_msgs/TFMessage.hpp" #include "estimator_pose.hpp" @@ -181,10 +179,6 @@ struct M20PointLioConfig { double processing_rate_hz; double pointcloud_rate_hz; double odometry_rate_hz; - double readiness_rate_hz; - double lidar_timeout_s; - double imu_timeout_s; - double estimate_timeout_s; double max_scan_duration_s; int max_cloud_points; double msr_freq; @@ -247,10 +241,6 @@ struct M20PointLioConfig { dimos::native::require_positive(processing_rate_hz, "processing_rate_hz"); dimos::native::require_positive(pointcloud_rate_hz, "pointcloud_rate_hz"); dimos::native::require_positive(odometry_rate_hz, "odometry_rate_hz"); - dimos::native::require_positive(readiness_rate_hz, "readiness_rate_hz"); - dimos::native::require_positive(lidar_timeout_s, "lidar_timeout_s"); - dimos::native::require_positive(imu_timeout_s, "imu_timeout_s"); - dimos::native::require_positive(estimate_timeout_s, "estimate_timeout_s"); dimos::native::require_positive(max_scan_duration_s, "max_scan_duration_s"); dimos::native::require_positive(msr_freq, "msr_freq"); dimos::native::require_positive(main_freq, "main_freq"); @@ -287,10 +277,6 @@ M20PointLioConfig parse_m20_pointlio_config(Config& config) { result.processing_rate_hz = config.take("processing_rate_hz"); result.pointcloud_rate_hz = config.take("pointcloud_rate_hz"); result.odometry_rate_hz = config.take("odometry_rate_hz"); - result.readiness_rate_hz = config.take("readiness_rate_hz"); - result.lidar_timeout_s = config.take("lidar_timeout_s"); - result.imu_timeout_s = config.take("imu_timeout_s"); - result.estimate_timeout_s = config.take("estimate_timeout_s"); result.max_scan_duration_s = config.take("max_scan_duration_s"); result.max_cloud_points = config.take("max_cloud_points"); result.msr_freq = config.take("msr_freq"); @@ -354,11 +340,8 @@ class M20PointLio : public Module { void build(Builder& builder, Config& config) override { cfg_ = parse_m20_pointlio_config(config); - lidar_ready_ = builder.output("lidar_ready"); - localization_ready_ = builder.output("localization_ready"); lidar_ = builder.output("lidar"); odom_ = builder.output("odom"); - odometry_ = builder.output("odometry"); tf_ = builder.output("tf"); process_period_ = std::chrono::duration_cast( @@ -367,8 +350,6 @@ class M20PointLio : public Module { std::chrono::duration(1.0 / cfg_.pointcloud_rate_hz)); odometry_period_ = std::chrono::duration_cast( std::chrono::duration(1.0 / cfg_.odometry_rate_hz)); - readiness_period_ = std::chrono::duration_cast( - std::chrono::duration(1.0 / cfg_.readiness_rate_hz)); } void setup() override { @@ -465,7 +446,6 @@ class M20PointLio : public Module { const auto now = Clock::now(); last_pointcloud_publish_ = now; last_odometry_publish_ = now; - last_readiness_publish_ = now - readiness_period_; processing_thread_ = std::thread([this]() { processing_loop(); }); spin_thread_ = std::thread([this]() { executor_->spin(); }); logging::info("M20 Point-LIO started", @@ -496,26 +476,16 @@ class M20PointLio : public Module { if (processing_thread_.joinable()) { processing_thread_.join(); } - std_msgs::Bool ready; - ready.data = 0; - lidar_ready_.publish(ready); - localization_ready_.publish(ready); point_lio_.reset(); } private: void on_lidar(const sensor_msgs::msg::PointCloud2& source) { if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; - const auto received_at = Clock::now(); bool feed_reserved = false; try { const auto offsets = validate_m20_cloud(source); - { - std::lock_guard lock(health_mutex_); - last_lidar_received_at_ = received_at; - have_lidar_ = true; - } std::unique_lock callback_lock(lidar_callback_mutex_, std::try_to_lock); @@ -694,7 +664,6 @@ class M20PointLio : public Module { void on_imu(const sensor_msgs::msg::Imu& source) { if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; - const auto received_at = Clock::now(); const double timestamp = header_seconds(source.header); if (!std::isfinite(timestamp) || timestamp <= 0.0 || !std::isfinite(source.angular_velocity.x) || @@ -737,33 +706,10 @@ class M20PointLio : public Module { (kStandardGravityMps2 * kStandardGravityMps2); } - { - std::lock_guard lock(health_mutex_); - last_imu_received_at_ = received_at; - have_imu_ = true; - } point_lio_->feed_imu(message); last_imu_sensor_time_ = timestamp; } - bool lidar_health_is_fresh(Clock::time_point now) const { - std::lock_guard lock(health_mutex_); - return have_lidar_ && - now - last_lidar_received_at_ <= - std::chrono::duration(cfg_.lidar_timeout_s); - } - - bool localization_health_is_fresh(Clock::time_point now) const { - std::lock_guard lock(health_mutex_); - return have_lidar_ && have_imu_ && have_estimate_ && - now - last_lidar_received_at_ <= - std::chrono::duration(cfg_.lidar_timeout_s) && - now - last_imu_received_at_ <= - std::chrono::duration(cfg_.imu_timeout_s) && - now - last_estimate_advanced_at_ <= - std::chrono::duration(cfg_.estimate_timeout_s); - } - void processing_loop() { while (!stopping_.load(std::memory_order_acquire)) { const auto iteration_started = Clock::now(); @@ -795,14 +741,10 @@ class M20PointLio : public Module { } } - const auto now = Clock::now(); bool estimate_advanced = false; if (have_estimate && std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { - std::lock_guard lock(health_mutex_); - if (!have_estimate_ || estimate_stamp > last_estimate_sensor_time_) { - last_estimate_sensor_time_ = estimate_stamp; - last_estimate_advanced_at_ = now; - have_estimate_ = true; + if (estimate_stamp > last_processed_estimate_stamp_) { + last_processed_estimate_stamp_ = estimate_stamp; estimate_advanced = true; } } @@ -811,10 +753,6 @@ class M20PointLio : public Module { estimator_initialized_ = true; lidar_feed_pending_ = false; } - if (now - last_readiness_publish_ >= readiness_period_) { - publish_readiness(now); - last_readiness_publish_ = now; - } const auto elapsed = Clock::now() - iteration_started; if (elapsed < process_period_) { @@ -823,39 +761,6 @@ class M20PointLio : public Module { } } - void publish_readiness(Clock::time_point now) { - const bool lidar_ready = lidar_health_is_fresh(now); - const bool localization_ready = localization_health_is_fresh(now); - std_msgs::Bool lidar_message; - lidar_message.data = static_cast(lidar_ready); - lidar_ready_.publish(lidar_message); - std_msgs::Bool localization_message; - localization_message.data = static_cast(localization_ready); - localization_ready_.publish(localization_message); - - const int8_t current_lidar = lidar_ready ? 1 : 0; - const int8_t previous_lidar = - lidar_readiness_state_.exchange(current_lidar, std::memory_order_acq_rel); - if (current_lidar != previous_lidar) { - if (lidar_ready) { - logging::info("M20 Point-LIO lidar input is ready"); - } else { - logging::warn("M20 Point-LIO lidar input is not ready"); - } - } - - const int8_t current = localization_ready ? 1 : 0; - const int8_t previous = - readiness_state_.exchange(current, std::memory_order_acq_rel); - if (current != previous) { - if (localization_ready) { - logging::info("M20 Point-LIO localization is ready"); - } else { - logging::warn("M20 Point-LIO localization is not ready"); - } - } - } - void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double timestamp) { const auto count = static_cast(cloud->size()); auto output = dimos::make_xyzi_cloud(cfg_.base_frame, timestamp, count); @@ -870,53 +775,34 @@ class M20PointLio : public Module { } void publish_odometry(const custom_messages::Odometry& source, double timestamp) { - nav_msgs::Odometry output; - output.header = dimos::make_header(cfg_.world_frame, timestamp); - output.child_frame_id = cfg_.base_frame; - output.pose.pose.position.x = source.pose.pose.position.x; - output.pose.pose.position.y = source.pose.pose.position.y; - output.pose.pose.position.z = source.pose.pose.position.z; - output.pose.pose.orientation.x = source.pose.pose.orientation.x; - output.pose.pose.orientation.y = source.pose.pose.orientation.y; - output.pose.pose.orientation.z = source.pose.pose.orientation.z; - output.pose.pose.orientation.w = source.pose.pose.orientation.w; - output.twist.twist.linear.x = source.twist.twist.linear.x; - output.twist.twist.linear.y = source.twist.twist.linear.y; - output.twist.twist.linear.z = source.twist.twist.linear.z; - output.twist.twist.angular.x = source.twist.twist.angular.x; - output.twist.twist.angular.y = source.twist.twist.angular.y; - output.twist.twist.angular.z = source.twist.twist.angular.z; - for (int index = 0; index < 36; ++index) { - output.pose.covariance[index] = source.pose.covariance[index]; - output.twist.covariance[index] = source.twist.covariance[index]; - } - geometry_msgs::PoseStamped pose; - pose.header = output.header; - pose.pose = output.pose.pose; + pose.header = dimos::make_header(cfg_.world_frame, timestamp); + pose.pose.position.x = source.pose.pose.position.x; + pose.pose.position.y = source.pose.pose.position.y; + pose.pose.position.z = source.pose.pose.position.z; + pose.pose.orientation.x = source.pose.pose.orientation.x; + pose.pose.orientation.y = source.pose.pose.orientation.y; + pose.pose.orientation.z = source.pose.pose.orientation.z; + pose.pose.orientation.w = source.pose.pose.orientation.w; geometry_msgs::TransformStamped transform; - transform.header = output.header; + transform.header = pose.header; transform.child_frame_id = cfg_.base_frame; - transform.transform.translation.x = output.pose.pose.position.x; - transform.transform.translation.y = output.pose.pose.position.y; - transform.transform.translation.z = output.pose.pose.position.z; - transform.transform.rotation = output.pose.pose.orientation; + transform.transform.translation.x = pose.pose.position.x; + transform.transform.translation.y = pose.pose.position.y; + transform.transform.translation.z = pose.pose.position.z; + transform.transform.rotation = pose.pose.orientation; tf2_msgs::TFMessage transforms; transforms.transforms_length = 1; transforms.transforms.push_back(std::move(transform)); - odometry_.publish(output); odom_.publish(pose); tf_.publish(transforms); } M20PointLioConfig cfg_; - Output lidar_ready_; - Output localization_ready_; Output lidar_; Output odom_; - Output odometry_; Output tf_; std::unique_ptr point_lio_; @@ -932,23 +818,13 @@ class M20PointLio : public Module { Clock::duration process_period_{}; Clock::duration pointcloud_period_{}; Clock::duration odometry_period_{}; - Clock::duration readiness_period_{}; Clock::time_point last_pointcloud_publish_{}; Clock::time_point last_odometry_publish_{}; - Clock::time_point last_readiness_publish_{}; double last_pointcloud_stamp_ = 0.0; double last_odometry_stamp_ = 0.0; double last_lidar_sensor_time_ = 0.0; double last_imu_sensor_time_ = 0.0; - - mutable std::mutex health_mutex_; - Clock::time_point last_lidar_received_at_{}; - Clock::time_point last_imu_received_at_{}; - Clock::time_point last_estimate_advanced_at_{}; - double last_estimate_sensor_time_ = 0.0; - bool have_lidar_ = false; - bool have_imu_ = false; - bool have_estimate_ = false; + double last_processed_estimate_stamp_ = 0.0; std::atomic stopping_{false}; mutable std::mutex lidar_feed_mutex_; std::mutex lidar_callback_mutex_; @@ -958,8 +834,6 @@ class M20PointLio : public Module { std::atomic busy_lidar_drops_{0}; std::atomic logged_cloud_contract_{false}; std::atomic logged_cloud_sampling_{false}; - std::atomic lidar_readiness_state_{-1}; - std::atomic readiness_state_{-1}; }; int main() { diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py index aa8c14b1ce..a1e3642df6 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/module.py +++ b/dimos/robot/deeprobotics/m20/pointlio/module.py @@ -23,9 +23,7 @@ from dimos.core.native_module import NativeModule, NativeModuleConfig from dimos.core.stream import Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.tf2_msgs.TFMessage import TFMessage from dimos.spec import perception @@ -41,6 +39,7 @@ class M20PointLioConfig(NativeModuleConfig): stdin_config: bool = True extra_env: dict[str, str] = Field( default_factory=lambda: { + "FASTRTPS_DEFAULT_PROFILES_FILE": "/opt/robot/fastdds.xml", "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", } @@ -57,10 +56,6 @@ class M20PointLioConfig(NativeModuleConfig): processing_rate_hz: float = Field(default=1000.0, gt=0.0) pointcloud_rate_hz: float = Field(default=10.0, gt=0.0) odometry_rate_hz: float = Field(default=50.0, gt=0.0) - readiness_rate_hz: float = Field(default=10.0, gt=0.0) - lidar_timeout_s: float = Field(default=0.5, gt=0.0) - imu_timeout_s: float = Field(default=0.5, gt=0.0) - estimate_timeout_s: float = Field(default=0.5, gt=0.0) max_scan_duration_s: float = Field(default=0.2, gt=0.0) # Live merged M20 frames contain roughly 100k returns. Point-LIO cannot # process that rate in real time on the RK3588, so the native adapter @@ -130,7 +125,7 @@ class M20PointLioConfig(NativeModuleConfig): debug: bool = False -class M20PointLio(NativeModule, perception.Lidar, perception.Odometry): +class M20PointLio(NativeModule, perception.Lidar): """Run the pinned Point-LIO core directly on the M20's ROS sensor topics. The native process subscribes to the merged ``base_link`` cloud and 200 Hz @@ -140,11 +135,8 @@ class M20PointLio(NativeModule, perception.Lidar, perception.Odometry): config: M20PointLioConfig - lidar_ready: Out[Bool] - localization_ready: Out[Bool] lidar: Out[PointCloud2] odom: Out[PoseStamped] - odometry: Out[Odometry] tf: Out[TFMessage] diff --git a/dimos/robot/deeprobotics/m20/pointlio/test_module.py b/dimos/robot/deeprobotics/m20/pointlio/test_module.py deleted file mode 100644 index c0fc068610..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/test_module.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Configuration contract tests for the native M20 Point-LIO adapter.""" - -from pydantic import ValidationError -import pytest - -from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLioConfig - - -def test_pointlio_matches_verified_m20_sensor_contract() -> None: - config = M20PointLioConfig() - - assert config.base_frame == "base_link" - assert config.world_frame == "odom" - assert config.scan_line == 192 - assert config.scan_rate == 10 - assert config.msr_freq == 200.0 - assert config.imu_time_inte == 0.005 - assert config.lidar_timeout_s == 0.5 - assert config.imu_timeout_s == 0.5 - assert config.estimate_timeout_s == 0.5 - assert config.max_cloud_points == 20_000 - assert config.extrinsic_t == [0.0, 0.0, 0.0] - assert config.extrinsic_r == [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - - -def test_pointlio_rejects_cloud_limit_above_native_static_capacity() -> None: - with pytest.raises(ValidationError): - M20PointLioConfig(max_cloud_points=100_001) diff --git a/dimos/robot/deeprobotics/m20/test_connection.py b/dimos/robot/deeprobotics/m20/test_connection.py deleted file mode 100644 index a2db2e1241..0000000000 --- a/dimos/robot/deeprobotics/m20/test_connection.py +++ /dev/null @@ -1,401 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Behavior tests for the guarded M20 command surface.""" - -from collections.abc import Callable, Iterator -import json -import math -import struct - -import pytest -from pytest_mock import MockerFixture - -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.std_msgs.Bool import Bool -from dimos.protocol.rpc.pubsubrpc import LCMRPC -from dimos.robot.deeprobotics.m20.connection import ( - GAIT_BASIC, - GAIT_FLAT_AGILE, - M20Connection, - M20ConnectionConfig, - sanitize_twist, -) - - -@pytest.fixture -def connection_factory( - mocker: MockerFixture, -) -> Iterator[Callable[..., M20Connection]]: - """Create connections and close their transport resources after each test.""" - del mocker # Keep mocked methods installed until connection.stop() completes. - connections: list[M20Connection] = [] - - def create(**kwargs: float) -> M20Connection: - connection = M20Connection(rpc_transport=LCMRPC, **kwargs) - connections.append(connection) - return connection - - yield create - - for connection in connections: - connection.stop() - - -def test_sanitize_twist_bounds_planar_command() -> None: - config = M20ConnectionConfig( - max_linear_x=0.5, - max_linear_y=0.25, - max_angular_z=0.75, - ) - command = Twist( - linear=Vector3(2.0, -2.0, 4.0), - angular=Vector3(1.0, 2.0, -3.0), - ) - - result = sanitize_twist(command, config) - - assert result == Twist( - linear=Vector3(0.5, -0.25, 0.0), - angular=Vector3(0.0, 0.0, -0.75), - ) - - -def test_sanitize_twist_rejects_nonfinite_command() -> None: - config = M20ConnectionConfig() - command = Twist(linear=Vector3(math.nan, 0.0, 0.0)) - - result = sanitize_twist(command, config) - - assert result == Twist.zero() - - -def test_connection_blocks_commands_until_explicitly_armed( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - command = Twist(linear=Vector3(0.2, 0.0, 0.0)) - - accepted = connection.move(command) - - assert accepted is False - publish.assert_called_once_with(Twist.zero()) - - -def test_connection_forwards_bounded_command_after_arm( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory( - max_linear_x=0.4, - max_linear_y=0.3, - max_angular_z=0.6, - ) - safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - armed_publish = mocker.patch.object(connection.armed, "publish") - command = Twist(linear=Vector3(0.7, -0.4, 2.0), angular=Vector3(1.0, 2.0, 0.9)) - - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - connection.arm() - accepted = connection.move(command) - - assert accepted is True - armed_publish.assert_called_once() - assert armed_publish.call_args.args[0].data is True - safe_publish.assert_called_once_with( - Twist(linear=Vector3(0.4, -0.3, 0.0), angular=Vector3(0.0, 0.0, 0.6)) - ) - - -def test_disarm_publishes_zero_and_blocks_following_commands( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - armed_publish = mocker.patch.object(connection.armed, "publish") - - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - connection.arm() - safe_publish.reset_mock() - armed_publish.reset_mock() - connection.disarm() - accepted = connection.move(Twist(linear=Vector3(0.2, 0.0, 0.0))) - - assert accepted is False - assert safe_publish.call_args_list == [mocker.call(Twist.zero()), mocker.call(Twist.zero())] - armed_publish.assert_called_once() - assert armed_publish.call_args.args[0].data is False - - -def test_connection_refuses_arm_until_native_bridge_is_ready( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - - accepted = connection.arm() - - assert accepted is False - assert connection.is_armed() is False - armed_publish.assert_not_called() - - -def test_connection_temporarily_inhibits_output_without_clearing_operator_arm( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - connection.arm() - safe_publish.reset_mock() - armed_publish.reset_mock() - - connection._on_command_ready(Bool(False)) - - assert connection.is_command_ready() is False - assert connection.is_armed() is True - safe_publish.assert_called_once_with(Twist.zero()) - armed_publish.assert_not_called() - - connection._on_command_ready(Bool(True)) - assert connection.move(Twist(linear=Vector3(0.2, 0.0, 0.0))) is True - assert safe_publish.call_args.args[0].linear.x == 0.2 - - -def test_standup_runs_complete_control_start_sequence( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - enter_navigation_mode = mocker.patch.object( - connection, "enter_navigation_mode", return_value=True - ) - ensure_rl_control = mocker.patch.object(connection, "_ensure_rl_control", return_value=True) - set_gait = mocker.patch.object(connection, "_set_gait_and_wait", return_value=True) - wait_ready = mocker.patch.object(connection, "_wait_for_control_readiness", return_value=True) - arm = mocker.patch.object(connection, "arm", return_value=True) - - assert connection.standup() is True - - enter_navigation_mode.assert_called_once_with() - ensure_rl_control.assert_called_once_with() - assert set_gait.call_args_list == [mocker.call(GAIT_BASIC), mocker.call(GAIT_FLAT_AGILE)] - wait_ready.assert_called_once_with(15.0) - arm.assert_called_once_with() - - -def test_standup_stops_when_navigation_usage_mode_is_rejected( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - mocker.patch.object(connection, "enter_navigation_mode", return_value=False) - ensure_rl_control = mocker.patch.object(connection, "_ensure_rl_control") - - assert connection.standup() is False - - ensure_rl_control.assert_not_called() - - -def test_navigation_mode_uses_documented_basic_server_apdu( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - response_payload = json.dumps( - { - "PatrolDevice": { - "Type": 1101, - "Command": 5, - "Items": {"ErrorCode": 0}, - } - } - ).encode() - response_header = struct.pack( - "<4sHHB7s", - bytes.fromhex("eb91eb90"), - len(response_payload), - 0, - 1, - b"\0" * 7, - ) - basic_server_socket = mocker.MagicMock() - basic_server_socket.__enter__.return_value = basic_server_socket - basic_server_socket.recv.side_effect = [response_header, response_payload] - create_connection = mocker.patch( - "dimos.robot.deeprobotics.m20.connection.socket.create_connection", - return_value=basic_server_socket, - ) - - assert connection.enter_navigation_mode() is True - - create_connection.assert_called_once_with(("10.21.31.103", 30001), timeout=3.0) - sent = basic_server_socket.sendall.call_args.args[0] - magic, payload_length, message_id, encoding, reserved = struct.unpack("<4sHHB7s", sent[:16]) - request = json.loads(sent[16:]) - assert (magic, payload_length, message_id, encoding, reserved) == ( - bytes.fromhex("eb91eb90"), - len(sent) - 16, - 0, - 1, - b"\0" * 7, - ) - assert request["PatrolDevice"]["Type"] == 1101 - assert request["PatrolDevice"]["Command"] == 5 - assert request["PatrolDevice"]["Items"] == {"Mode": 1} - - -def test_low_level_motion_and_gait_endpoints_publish_vendor_commands( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - motion_publish = mocker.patch.object(connection.motion_state_cmd, "publish") - gait_publish = mocker.patch.object(connection.gait_cmd, "publish") - wait = mocker.patch.object(connection, "_wait_for_motion_state", return_value=True) - - assert connection.enter_rl_control() is True - assert connection.set_navigation_gait() is True - assert connection.liedown() is True - - assert [call.args[0].data for call in motion_publish.call_args_list] == [17, 4] - assert gait_publish.call_args.args[0].data == GAIT_FLAT_AGILE - wait.assert_called_once_with(17, 5.0) - - -def test_rl_control_transition_follows_vendor_stand_sequence( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - motion_publish = mocker.patch.object(connection.motion_state_cmd, "publish") - wait = mocker.patch.object(connection, "_wait_for_motion_state", return_value=True) - - assert connection._ensure_rl_control() is True - - assert [call.args[0].data for call in motion_publish.call_args_list] == [1, 17] - assert wait.call_args_list == [mocker.call(1, 12.0), mocker.call(17, 5.0)] - - -def test_rejects_unknown_gait( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - publish = mocker.patch.object(connection.gait_cmd, "publish") - - assert connection.set_gait(12345) is False - publish.assert_not_called() - - -def test_lidar_diagnostics_do_not_gate_robot_control( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - - accepted = connection.arm() - - assert accepted is True - assert connection.is_lidar_ready() is False - assert connection.is_armed() is True - armed_publish.assert_called_once() - - -def test_connection_reports_lidar_recovery( - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - - connection._on_lidar_ready(Bool(True)) - - assert connection.is_lidar_ready() is True - - -def test_lidar_staleness_does_not_clear_operator_arm( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - connection.arm() - safe_publish.reset_mock() - armed_publish.reset_mock() - - connection._on_lidar_ready(Bool(False)) - - assert connection.is_lidar_ready() is False - assert connection.is_armed() is True - safe_publish.assert_not_called() - armed_publish.assert_not_called() - - -def test_pointlio_diagnostics_do_not_gate_robot_control( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_lidar_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - - accepted = connection.arm() - - assert accepted is True - assert connection.is_localization_ready() is False - armed_publish.assert_called_once() - - -def test_pointlio_staleness_does_not_clear_operator_arm( - mocker: MockerFixture, - connection_factory: Callable[..., M20Connection], -) -> None: - connection = connection_factory() - safe_publish = mocker.patch.object(connection.safe_cmd_vel, "publish") - armed_publish = mocker.patch.object(connection.armed, "publish") - connection._on_lidar_ready(Bool(True)) - connection._on_localization_ready(Bool(True)) - connection._on_command_ready(Bool(True)) - assert connection.arm() is True - safe_publish.reset_mock() - armed_publish.reset_mock() - - connection._on_localization_ready(Bool(False)) - - assert connection.is_localization_ready() is False - assert connection.is_armed() is True - safe_publish.assert_not_called() - armed_publish.assert_not_called() diff --git a/native/cpp/tests/test_config.cpp b/native/cpp/tests/test_config.cpp index 56965796e3..29bfdf33b7 100644 --- a/native/cpp/tests/test_config.cpp +++ b/native/cpp/tests/test_config.cpp @@ -94,27 +94,6 @@ TEST_CASE("a non-object config is rejected") { CHECK_THROWS_AS(Config(json::array({1, 2})), std::runtime_error); } -TEST_CASE("take reads explicit fields and preserves strict consumption") { - Config cfg(json{{"count", 3}, {"enabled", true}, {"name", "m20"}}); - - CHECK(cfg.take("count") == 3); - CHECK(cfg.take("enabled")); - CHECK(cfg.take("name") == "m20"); - CHECK_NOTHROW(cfg.enforce_all_consumed()); -} - -TEST_CASE("take rejects missing, wrong-typed, and unconsumed fields") { - Config missing(json::object()); - CHECK_THROWS_AS(missing.take("rate"), std::runtime_error); - - Config wrong_type(json{{"enabled", 1}}); - CHECK_THROWS_AS(wrong_type.take("enabled"), std::runtime_error); - - Config unknown(json{{"known", 1}, {"extra", 2}}); - CHECK(unknown.take("known") == 1); - CHECK_THROWS_AS(unknown.enforce_all_consumed(), std::runtime_error); -} - TEST_CASE("parse deserializes a typed config struct") { Config cfg(json{{"value", 5}, {"name", "lidar"}}); RangedCfg c = cfg.parse(); From e3d6473f2ce7e43413c6f616a4a109a1656f6411 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 18:55:16 +0800 Subject: [PATCH 06/15] refactor(robot): simplify M20 native lifecycle --- .../m20/blueprints/m20_kronknav.py | 7 +- .../deeprobotics/m20/bridge/cpp/main.cpp | 21 ++-- dimos/robot/deeprobotics/m20/bridge/module.py | 1 - dimos/robot/deeprobotics/m20/connection.py | 79 +++--------- .../deeprobotics/m20/pointlio/cpp/main.cpp | 112 ++++++++---------- .../robot/deeprobotics/m20/pointlio/module.py | 1 - 6 files changed, 78 insertions(+), 143 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 81aa16f831..fb990c12de 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -117,17 +117,12 @@ def _m20_rerun_blueprint() -> Any: deeprobotics_m20_kronknav_control = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), M20ROSBridge.blueprint( - enable_command_output=True, max_linear_x=MAX_LINEAR_X_M_S, max_linear_y=MAX_LINEAR_Y_M_S, max_angular_z=MAX_ANGULAR_Z_RAD_S, ), M20PointLio.blueprint(), - M20Connection.blueprint( - max_linear_x=MAX_LINEAR_X_M_S, - max_linear_y=MAX_LINEAR_Y_M_S, - max_angular_z=MAX_ANGULAR_Z_RAD_S, - ), + M20Connection.blueprint(), RayTracingVoxelMap.blueprint( voxel_size=VOXEL_SIZE_M, max_range=25.0, diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp index 9b739fc333..dfc0e84147 100644 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp @@ -156,7 +156,6 @@ struct M20ROSBridgeConfig { std::string gait_topic; std::string hes_status_topic; std::string node_name; - bool enable_command_output; double command_rate_hz; double command_timeout_s; double safety_timeout_s; @@ -188,7 +187,6 @@ M20ROSBridgeConfig parse_m20_config(Config& config) { result.gait_topic = config.take("gait_topic"); result.hes_status_topic = config.take("hes_status_topic"); result.node_name = config.take("node_name"); - result.enable_command_output = config.take("enable_command_output"); result.command_rate_hz = config.take("command_rate_hz"); result.command_timeout_s = config.take("command_timeout_s"); result.safety_timeout_s = config.take("safety_timeout_s"); @@ -236,14 +234,12 @@ class M20ROSBridge : public Module { rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(), [this](drdds::msg::MotionInfo::SharedPtr msg) { on_motion_info(*msg); }); - if (cfg_.enable_command_output) { - nav_cmd_publisher_ = node_->create_publisher( - cfg_.nav_cmd_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - motion_state_publisher_ = node_->create_publisher( - cfg_.motion_state_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - gait_publisher_ = node_->create_publisher( - cfg_.gait_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - } + nav_cmd_publisher_ = node_->create_publisher( + cfg_.nav_cmd_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); + motion_state_publisher_ = node_->create_publisher( + cfg_.motion_state_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); + gait_publisher_ = node_->create_publisher( + cfg_.gait_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); const auto period = std::chrono::duration(1.0 / cfg_.command_rate_hz); timer_ = node_->create_wall_timer( @@ -254,8 +250,7 @@ class M20ROSBridge : public Module { executor_->add_node(node_); spin_thread_ = std::thread([this]() { executor_->spin(); }); - logging::info("M20 command/state ROS bridge started", - {logging::Field("command_output", cfg_.enable_command_output)}); + logging::info("M20 command/state ROS bridge started"); } void teardown() override { @@ -346,7 +341,7 @@ class M20ROSBridge : public Module { bool safety_ready(Clock::time_point now) const { std::lock_guard lock(state_mutex_); - if (!cfg_.enable_command_output || !have_motion_info_) { + if (!have_motion_info_) { return false; } const auto timeout = std::chrono::duration(cfg_.safety_timeout_s); diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py index e321ebada6..51e09ee1f1 100644 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ b/dimos/robot/deeprobotics/m20/bridge/module.py @@ -58,7 +58,6 @@ class M20ROSBridgeConfig(NativeModuleConfig): hes_status_topic: str = "/HES_STATUS" node_name: str = "dimos_m20_bridge" - enable_command_output: bool = False command_rate_hz: float = Field(default=10.0, gt=0.0) command_timeout_s: float = Field(default=0.4, gt=0.0) safety_timeout_s: float = Field(default=2.5, gt=0.0) diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index 9057db83ea..f3f9e8ed57 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -18,11 +18,9 @@ from datetime import datetime import json -import math import socket import struct from threading import Condition, RLock -import time from typing import Any from pydantic import Field @@ -32,16 +30,11 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.std_msgs.Int32 import Int32 from dimos.msgs.std_msgs.UInt32 import UInt32 -from dimos.robot.deeprobotics.m20.constants import ( - MAX_ANGULAR_Z_RAD_S, - MAX_LINEAR_X_M_S, - MAX_LINEAR_Y_M_S, -) from dimos.utils.logging_config import setup_logger +from dimos.utils.sequential_ids import SequentialIds logger = setup_logger() @@ -69,11 +62,8 @@ class M20ConnectionConfig(ModuleConfig): - """Limits for commands sent to the M20's high-level navigation interface.""" + """State-transition and basic-server settings for M20 control.""" - max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) - max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) - max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) require_command_ready: bool = True stand_timeout_s: float = Field(default=12.0, gt=0.0) rl_control_timeout_s: float = Field(default=5.0, gt=0.0) @@ -84,32 +74,14 @@ class M20ConnectionConfig(ModuleConfig): basic_server_timeout_s: float = Field(default=3.0, gt=0.0) -def _clamp(value: float, limit: float) -> float: - return max(-limit, min(limit, value)) - - -def sanitize_twist(twist: Twist, config: M20ConnectionConfig) -> Twist: - """Return a finite, planar Twist bounded by the configured M20 limits.""" - values = (twist.linear.x, twist.linear.y, twist.angular.z) - if not all(math.isfinite(value) for value in values): - return Twist.zero() - return Twist( - linear=Vector3( - _clamp(twist.linear.x, config.max_linear_x), - _clamp(twist.linear.y, config.max_linear_y), - 0.0, - ), - angular=Vector3(0.0, 0.0, _clamp(twist.angular.z, config.max_angular_z)), - ) - - class M20Connection(Module): """Expose the planner-facing M20 command surface with an explicit operator arm. The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, - rejects it until ``standup()`` has armed control, bounds planar commands, and - emits ``safe_cmd_vel`` for the robot-local bridge. + rejects it until ``standup()`` has armed control, and emits ``safe_cmd_vel`` + for the robot-local bridge. The native bridge is the single command-validation + and velocity-clamping boundary. ``standup()`` is the normal one-call operator entry point: it completes the vendor state and gait transitions, waits for the guarded command path, and @@ -136,7 +108,7 @@ def __init__(self, **kwargs: Any) -> None: self._command_ready = False self._motion_state: int | None = None self._gait_state: int | None = None - self._basic_server_message_id = 0 + self._basic_server_message_ids = SequentialIds() @rpc def start(self) -> None: @@ -155,7 +127,7 @@ def stop(self) -> None: @rpc def arm(self) -> bool: - """Allow bounded planner commands to reach the M20 ROS bridge.""" + """Allow planner commands to reach the M20 ROS bridge.""" with self._lock: if self.config.require_command_ready and not self._command_ready: logger.warning("M20 command gate refused arm: robot control path is not ready") @@ -189,7 +161,7 @@ def is_command_ready(self) -> bool: @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Forward a bounded planar velocity when armed. + """Forward velocity to the native validation boundary when armed. ``duration`` is accepted for connection compatibility. Command lifetime is enforced by the native bridge's monotonic watchdog. @@ -197,8 +169,7 @@ def move(self, twist: Twist, duration: float = 0.0) -> bool: del duration with self._lock: enabled = self._armed and (self._command_ready or not self.config.require_command_ready) - command = sanitize_twist(twist, self.config) if enabled else Twist.zero() - self.safe_cmd_vel.publish(command) + self.safe_cmd_vel.publish(twist if enabled else Twist.zero()) return enabled @rpc @@ -363,9 +334,7 @@ def _basic_server_request( if len(payload) > 0xFFFF: raise ValueError("M20 basic_server payload exceeds the APDU limit") - with self._lock: - message_id = self._basic_server_message_id - self._basic_server_message_id = (message_id + 1) & 0xFFFF + message_id = self._basic_server_message_ids.next() & 0xFFFF header = _BASIC_SERVER_HEADER.pack( _BASIC_SERVER_MAGIC, len(payload), @@ -404,34 +373,20 @@ def _basic_server_request( return decoded if isinstance(decoded, dict) else {} def _wait_for_motion_state(self, expected: int, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s with self._state_condition: - while self._motion_state != expected: - remaining = deadline - time.monotonic() - if remaining <= 0.0: - return False - self._state_condition.wait(remaining) - return True + return self._state_condition.wait_for( + lambda: self._motion_state == expected, timeout=timeout_s + ) def _wait_for_gait_state(self, expected: int, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s with self._state_condition: - while self._gait_state != expected: - remaining = deadline - time.monotonic() - if remaining <= 0.0: - return False - self._state_condition.wait(remaining) - return True + return self._state_condition.wait_for( + lambda: self._gait_state == expected, timeout=timeout_s + ) def _wait_for_control_readiness(self, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s with self._state_condition: - while not self._command_ready: - remaining = deadline - time.monotonic() - if remaining <= 0.0: - return False - self._state_condition.wait(remaining) - return True + return self._state_condition.wait_for(lambda: self._command_ready, timeout=timeout_s) def _recv_exact(connection: socket.socket, size: int) -> bytes: diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp index 8e6ac4bee1..e1dcc19288 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp @@ -176,7 +176,6 @@ struct M20PointLioConfig { std::string node_name; std::string world_frame; std::string base_frame; - double processing_rate_hz; double pointcloud_rate_hz; double odometry_rate_hz; double max_scan_duration_s; @@ -238,7 +237,6 @@ struct M20PointLioConfig { require_nonempty(node_name, "node_name"); require_nonempty(world_frame, "world_frame"); require_nonempty(base_frame, "base_frame"); - dimos::native::require_positive(processing_rate_hz, "processing_rate_hz"); dimos::native::require_positive(pointcloud_rate_hz, "pointcloud_rate_hz"); dimos::native::require_positive(odometry_rate_hz, "odometry_rate_hz"); dimos::native::require_positive(max_scan_duration_s, "max_scan_duration_s"); @@ -274,7 +272,6 @@ M20PointLioConfig parse_m20_pointlio_config(Config& config) { result.node_name = config.take("node_name"); result.world_frame = config.take("world_frame"); result.base_frame = config.take("base_frame"); - result.processing_rate_hz = config.take("processing_rate_hz"); result.pointcloud_rate_hz = config.take("pointcloud_rate_hz"); result.odometry_rate_hz = config.take("odometry_rate_hz"); result.max_scan_duration_s = config.take("max_scan_duration_s"); @@ -345,7 +342,7 @@ class M20PointLio : public Module { tf_ = builder.output("tf"); process_period_ = std::chrono::duration_cast( - std::chrono::duration(1.0 / cfg_.processing_rate_hz)); + std::chrono::duration(1.0 / cfg_.main_freq)); pointcloud_period_ = std::chrono::duration_cast( std::chrono::duration(1.0 / cfg_.pointcloud_rate_hz)); odometry_period_ = std::chrono::duration_cast( @@ -446,7 +443,6 @@ class M20PointLio : public Module { const auto now = Clock::now(); last_pointcloud_publish_ = now; last_odometry_publish_ = now; - processing_thread_ = std::thread([this]() { processing_loop(); }); spin_thread_ = std::thread([this]() { executor_->spin(); }); logging::info("M20 Point-LIO started", {logging::Field("world_frame", cfg_.world_frame), @@ -473,12 +469,60 @@ class M20PointLio : public Module { if (rclcpp::ok()) { rclcpp::shutdown(); } - if (processing_thread_.joinable()) { - processing_thread_.join(); - } point_lio_.reset(); } + void handle() override { + while (!shutdown_requested()) { + const auto iteration_started = Clock::now(); + bool have_estimate = false; + double estimate_stamp = 0.0; + point_lio_->process(); + const auto pose = point_lio_->get_pose(); + have_estimate = dimos::has_estimate(pose); + if (have_estimate) { + const auto& source_odom = point_lio_->get_odometry(); + estimate_stamp = source_odom.header.stamp.toSec(); + const auto now = Clock::now(); + if (std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { + if (now - last_pointcloud_publish_ >= pointcloud_period_ && + estimate_stamp > last_pointcloud_stamp_) { + const auto cloud = point_lio_->get_body_cloud(); + if (cloud != nullptr && !cloud->empty()) { + publish_pointcloud(cloud, estimate_stamp); + last_pointcloud_stamp_ = estimate_stamp; + last_pointcloud_publish_ = now; + } + } + if (now - last_odometry_publish_ >= odometry_period_ && + estimate_stamp > last_odometry_stamp_) { + publish_odometry(source_odom, estimate_stamp); + last_odometry_stamp_ = estimate_stamp; + last_odometry_publish_ = now; + } + } + } + + bool estimate_advanced = false; + if (have_estimate && std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { + if (estimate_stamp > last_processed_estimate_stamp_) { + last_processed_estimate_stamp_ = estimate_stamp; + estimate_advanced = true; + } + } + if (estimate_advanced) { + std::lock_guard lock(lidar_feed_mutex_); + estimator_initialized_ = true; + lidar_feed_pending_ = false; + } + + const auto elapsed = Clock::now() - iteration_started; + if (elapsed < process_period_) { + std::this_thread::sleep_for(process_period_ - elapsed); + } + } + } + private: void on_lidar(const sensor_msgs::msg::PointCloud2& source) { if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; @@ -710,57 +754,6 @@ class M20PointLio : public Module { last_imu_sensor_time_ = timestamp; } - void processing_loop() { - while (!stopping_.load(std::memory_order_acquire)) { - const auto iteration_started = Clock::now(); - bool have_estimate = false; - double estimate_stamp = 0.0; - point_lio_->process(); - const auto pose = point_lio_->get_pose(); - have_estimate = dimos::has_estimate(pose); - if (have_estimate) { - const auto& source_odom = point_lio_->get_odometry(); - estimate_stamp = source_odom.header.stamp.toSec(); - const auto now = Clock::now(); - if (std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { - if (now - last_pointcloud_publish_ >= pointcloud_period_ && - estimate_stamp > last_pointcloud_stamp_) { - const auto cloud = point_lio_->get_body_cloud(); - if (cloud != nullptr && !cloud->empty()) { - publish_pointcloud(cloud, estimate_stamp); - last_pointcloud_stamp_ = estimate_stamp; - last_pointcloud_publish_ = now; - } - } - if (now - last_odometry_publish_ >= odometry_period_ && - estimate_stamp > last_odometry_stamp_) { - publish_odometry(source_odom, estimate_stamp); - last_odometry_stamp_ = estimate_stamp; - last_odometry_publish_ = now; - } - } - } - - bool estimate_advanced = false; - if (have_estimate && std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { - if (estimate_stamp > last_processed_estimate_stamp_) { - last_processed_estimate_stamp_ = estimate_stamp; - estimate_advanced = true; - } - } - if (estimate_advanced) { - std::lock_guard lock(lidar_feed_mutex_); - estimator_initialized_ = true; - lidar_feed_pending_ = false; - } - - const auto elapsed = Clock::now() - iteration_started; - if (elapsed < process_period_) { - std::this_thread::sleep_for(process_period_ - elapsed); - } - } - } - void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double timestamp) { const auto count = static_cast(cloud->size()); auto output = dimos::make_xyzi_cloud(cfg_.base_frame, timestamp, count); @@ -813,7 +806,6 @@ class M20PointLio : public Module { rclcpp::Subscription::SharedPtr lidar_subscription_; rclcpp::Subscription::SharedPtr imu_subscription_; std::thread spin_thread_; - std::thread processing_thread_; Clock::duration process_period_{}; Clock::duration pointcloud_period_{}; diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py index a1e3642df6..3fa3bb5cd8 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/module.py +++ b/dimos/robot/deeprobotics/m20/pointlio/module.py @@ -53,7 +53,6 @@ class M20PointLioConfig(NativeModuleConfig): node_name: str = "dimos_m20_pointlio" world_frame: str = "odom" base_frame: str = "base_link" - processing_rate_hz: float = Field(default=1000.0, gt=0.0) pointcloud_rate_hz: float = Field(default=10.0, gt=0.0) odometry_rate_hz: float = Field(default=50.0, gt=0.0) max_scan_duration_s: float = Field(default=0.2, gt=0.0) From af974b58b11050a0b293dfe1f74ebbfd6a357d9a Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 20:08:39 +0800 Subject: [PATCH 07/15] refactor(robot): align M20 control with KronkNav --- .../m20/blueprints/m20_kronknav.py | 3 - dimos/robot/deeprobotics/m20/connection.py | 185 +++++++----------- dimos/robot/deeprobotics/m20/constants.py | 11 +- 3 files changed, 82 insertions(+), 117 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index fb990c12de..30b9f6b3c5 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -42,8 +42,6 @@ VOXEL_SIZE_M = 0.1 PLANNER_VIZ_HZ = 0.0 -CRUISE_SPEED_M_S = 0.25 - def _render_global_map(msg: Any) -> Any: return msg.to_rerun() @@ -157,7 +155,6 @@ def _m20_rerun_blueprint() -> Any: ), DanHolonomicTC.blueprint( run_profile="walk", - speed_m_s=CRUISE_SPEED_M_S, control_frequency=10.0, ), MovementManager.blueprint(), diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index f3f9e8ed57..bc7ad15135 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -21,7 +21,7 @@ import socket import struct from threading import Condition, RLock -from typing import Any +from typing import Any, Literal from pydantic import Field from reactivex.disposable import Disposable @@ -38,21 +38,20 @@ logger = setup_logger() -MOTION_IDLE = 0 MOTION_STAND = 1 -MOTION_SOFT_ESTOP = 2 MOTION_SIT = 4 MOTION_RL_CONTROL = 17 GAIT_BASIC = 0x1001 -GAIT_STAIR_STANDARD = 0x1003 GAIT_FLAT_AGILE = 0x3002 GAIT_STAIR_AGILE = 0x3003 -SUPPORTED_GAITS = {GAIT_BASIC, GAIT_STAIR_STANDARD, GAIT_FLAT_AGILE, GAIT_STAIR_AGILE} +NavigationTerrain = Literal["flat", "stairs"] +_NAVIGATION_GAITS: dict[NavigationTerrain, int] = { + "flat": GAIT_FLAT_AGILE, + "stairs": GAIT_STAIR_AGILE, +} -USAGE_MODE_NORMAL = 0 USAGE_MODE_NAVIGATION = 1 -USAGE_MODE_ASSISTED = 2 _BASIC_SERVER_MAGIC = bytes.fromhex("eb91eb90") _BASIC_SERVER_JSON = 1 @@ -64,7 +63,6 @@ class M20ConnectionConfig(ModuleConfig): """State-transition and basic-server settings for M20 control.""" - require_command_ready: bool = True stand_timeout_s: float = Field(default=12.0, gt=0.0) rl_control_timeout_s: float = Field(default=5.0, gt=0.0) gait_timeout_s: float = Field(default=5.0, gt=0.0) @@ -75,18 +73,19 @@ class M20ConnectionConfig(ModuleConfig): class M20Connection(Module): - """Expose the planner-facing M20 command surface with an explicit operator arm. + """Expose the planner-facing M20 command and terrain surface. The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, - rejects it until ``standup()`` has armed control, and emits ``safe_cmd_vel`` + rejects it until ``standup()`` has enabled control, and emits ``safe_cmd_vel`` for the robot-local bridge. The native bridge is the single command-validation and velocity-clamping boundary. ``standup()`` is the normal one-call operator entry point: it completes the vendor state and gait transitions, waits for the guarded command path, and - arms velocity output. Lower-level RPCs remain available for recovery and - diagnostics. + enables velocity output. ``set_navigation_terrain()`` is the only exposed + vendor-specific control and selects one of the documented agile navigation + gaits without exposing raw gait values. """ config: M20ConnectionConfig @@ -96,7 +95,6 @@ class M20Connection(Module): motion_state: In[Int32] gait_state: In[UInt32] safe_cmd_vel: Out[Twist] - armed: Out[Bool] motion_state_cmd: Out[Int32] gait_cmd: Out[UInt32] @@ -104,10 +102,11 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._lock = RLock() self._state_condition = Condition(self._lock) - self._armed = False + self._commands_enabled = False self._command_ready = False self._motion_state: int | None = None self._gait_state: int | None = None + self._navigation_terrain: NavigationTerrain = "stairs" self._basic_server_message_ids = SequentialIds() @rpc @@ -118,75 +117,35 @@ def start(self) -> None: self.register_disposable(Disposable(self.motion_state.subscribe(self._on_motion_state))) self.register_disposable(Disposable(self.gait_state.subscribe(self._on_gait_state))) self.safe_cmd_vel.publish(Twist.zero()) - self.armed.publish(Bool(False)) @rpc def stop(self) -> None: - self.disarm() + self._disable_commands() super().stop() - @rpc - def arm(self) -> bool: - """Allow planner commands to reach the M20 ROS bridge.""" - with self._lock: - if self.config.require_command_ready and not self._command_ready: - logger.warning("M20 command gate refused arm: robot control path is not ready") - return False - self._armed = True - self.armed.publish(Bool(True)) - logger.warning("M20 command gate armed") - return True - - @rpc - def disarm(self) -> bool: - """Block commands and publish an immediate zero velocity.""" - with self._lock: - self._armed = False - self.safe_cmd_vel.publish(Twist.zero()) - self.armed.publish(Bool(False)) - logger.info("M20 command gate disarmed") - return True - - @rpc - def is_armed(self) -> bool: - """Return whether nonzero commands may pass through the gate.""" - with self._lock: - return self._armed - - @rpc - def is_command_ready(self) -> bool: - """Return whether the native bridge reports a live robot control path.""" - with self._lock: - return self._command_ready - @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Forward velocity to the native validation boundary when armed. + """Forward velocity to the native validation boundary when enabled. ``duration`` is accepted for connection compatibility. Command lifetime is enforced by the native bridge's monotonic watchdog. """ del duration with self._lock: - enabled = self._armed and (self._command_ready or not self.config.require_command_ready) + enabled = self._commands_enabled and self._command_ready self.safe_cmd_vel.publish(twist if enabled else Twist.zero()) return enabled @rpc def stop_movement(self) -> None: - """Publish an immediate zero velocity without changing the arm state.""" + """Publish an immediate zero velocity without disabling future commands.""" self.safe_cmd_vel.publish(Twist.zero()) @rpc def standup(self) -> bool: - """Bring the M20 to an armed, navigation-ready standing state.""" - return self.start_control() - - @rpc - def start_control(self) -> bool: - """Prepare and arm the complete M20 velocity-control path in one call.""" - self.disarm() - if not self.enter_navigation_mode(): + """Bring the M20 to a command-enabled, navigation-ready standing state.""" + self._disable_commands() + if not self._enter_navigation_mode(): logger.error("M20 basic_server rejected the navigation usage mode") return False if not self._ensure_rl_control(): @@ -197,71 +156,66 @@ def start_control(self) -> bool: if not self._set_gait_and_wait(GAIT_BASIC): logger.error("M20 did not confirm the Basic gait") return False - if not self._set_gait_and_wait(GAIT_FLAT_AGILE): - logger.error("M20 did not confirm the agile flat navigation gait") + with self._lock: + terrain = self._navigation_terrain + if not self._set_gait_and_wait(_NAVIGATION_GAITS[terrain]): + logger.error("M20 did not confirm the agile %s navigation gait", terrain) return False if not self._wait_for_control_readiness(self.config.control_ready_timeout_s): logger.error("M20 robot control path did not become ready") return False - return self.arm() - - @rpc - def enter_rl_control(self) -> bool: - """Command the standing M20 to enter RL Control for velocity operation.""" - self.disarm() - self.motion_state_cmd.publish(Int32(MOTION_RL_CONTROL)) - return self._wait_for_motion_state(MOTION_RL_CONTROL, self.config.rl_control_timeout_s) - - @rpc - def enter_navigation_mode(self) -> bool: - """Select the M20 usage mode in which ``/NAV_CMD`` is accepted.""" - return self._set_usage_mode(USAGE_MODE_NAVIGATION) + return self._enable_commands() @rpc def liedown(self) -> bool: - """Disarm velocity output and command the M20 to its Sit/prone state.""" - self.disarm() + """Disable velocity output and command the M20 to its Sit/prone state.""" + self._disable_commands() self.motion_state_cmd.publish(Int32(MOTION_SIT)) return True @rpc - def idle(self) -> bool: - """Disarm velocity output and command the M20 to Idle.""" - self.disarm() - self.motion_state_cmd.publish(Int32(MOTION_IDLE)) - return True - - @rpc - def soft_estop(self) -> bool: - """Disarm velocity output and request the vendor soft emergency stop state.""" - self.disarm() - self.motion_state_cmd.publish(Int32(MOTION_SOFT_ESTOP)) - return True + def set_navigation_terrain(self, terrain: NavigationTerrain) -> bool: + """Select the agile navigation gait for flat ground or stairs. - @rpc - def set_gait(self, gait: int) -> bool: - """Select a documented M20 gait while stationary in RL Control. - - Supported values are 0x1001 basic, 0x1003 standard stair, 0x3002 - agile flat, and 0x3003 agile stair. + Call this while the robot is stationary. In RL Control the switch is + applied immediately and confirmed through ``/MOTION_INFO``. Otherwise + the selection is retained and applied by the next ``standup()``. """ - if gait not in SUPPORTED_GAITS: + gait = _NAVIGATION_GAITS.get(terrain) + if gait is None: + logger.error("Unsupported M20 navigation terrain: %s", terrain) return False - self.gait_cmd.publish(UInt32(gait)) - return True - @rpc - def set_navigation_gait(self) -> bool: - """Select the vendor-recommended agile flat gait for autonomous navigation.""" - return self.set_gait(GAIT_FLAT_AGILE) + with self._lock: + if self._motion_state != MOTION_RL_CONTROL: + self._navigation_terrain = terrain + logger.info("M20 will select the agile %s gait on next standup", terrain) + return True + if self._gait_state == gait: + self._navigation_terrain = terrain + return True + restore_commands = self._commands_enabled + + self._disable_commands() + switched = self._set_gait_and_wait(gait) + if switched: + with self._lock: + self._navigation_terrain = terrain + logger.info("M20 selected the agile %s navigation gait", terrain) + else: + logger.error("M20 did not confirm the agile %s navigation gait", terrain) + + if restore_commands and not self._enable_commands(): + return False + return switched def _on_command_ready(self, msg: Bool) -> None: ready = bool(msg.data) with self._state_condition: - was_armed = self._armed + commands_were_enabled = self._commands_enabled self._command_ready = ready self._state_condition.notify_all() - if was_armed and not ready: + if commands_were_enabled and not ready: self.safe_cmd_vel.publish(Twist.zero()) logger.warning("M20 command output temporarily inhibited: robot control path is stale") @@ -295,13 +249,11 @@ def _set_gait_and_wait(self, gait: int) -> bool: self.gait_cmd.publish(UInt32(gait)) return self._wait_for_gait_state(gait, self.config.gait_timeout_s) - def _set_usage_mode(self, mode: int) -> bool: - if mode not in {USAGE_MODE_NORMAL, USAGE_MODE_NAVIGATION, USAGE_MODE_ASSISTED}: - return False + def _enter_navigation_mode(self) -> bool: response = self._basic_server_request( message_type=_BASIC_SERVER_MODE_TYPE, command=_BASIC_SERVER_MODE_COMMAND, - items={"Mode": mode}, + items={"Mode": USAGE_MODE_NAVIGATION}, ) try: error_code = int(response["PatrolDevice"]["Items"]["ErrorCode"]) @@ -313,6 +265,21 @@ def _set_usage_mode(self, mode: int) -> bool: return False return True + def _enable_commands(self) -> bool: + with self._lock: + if not self._command_ready: + logger.warning("M20 command gate refused enable: robot control path is not ready") + return False + self._commands_enabled = True + logger.info("M20 command output enabled") + return True + + def _disable_commands(self) -> None: + with self._lock: + self._commands_enabled = False + self.safe_cmd_vel.publish(Twist.zero()) + logger.info("M20 command output disabled") + def _basic_server_request( self, *, diff --git a/dimos/robot/deeprobotics/m20/constants.py b/dimos/robot/deeprobotics/m20/constants.py index 5e4464f3e9..036d55e221 100644 --- a/dimos/robot/deeprobotics/m20/constants.py +++ b/dimos/robot/deeprobotics/m20/constants.py @@ -30,8 +30,9 @@ ROTATION_DIAMETER_M = math.hypot(BODY_LENGTH_M, BODY_WIDTH_M) -# Conservative command bounds until direction signs, gait, latency, and -# stopping distance have been measured on the actual robot. -MAX_LINEAR_X_M_S = 0.3 -MAX_LINEAR_Y_M_S = 0.3 -MAX_ANGULAR_Z_RAD_S = 0.5 +# Documented upper command bounds across the supported agile navigation gaits. +# Navigation cruise speed remains controller-owned; these are only the final +# robot-facing envelope for /NAV_CMD. +MAX_LINEAR_X_M_S = 2.0 +MAX_LINEAR_Y_M_S = 1.0 +MAX_ANGULAR_Z_RAD_S = 2.0 From 0cd9eb0f10ae854b4c1937c2204ed5012d3ab0e1 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 21:12:25 +0800 Subject: [PATCH 08/15] fix(robot): preserve M20 3D planner paths --- dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 30b9f6b3c5..70dc1038f4 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -139,7 +139,7 @@ def _m20_rerun_blueprint() -> Any: wall_clearance_m=0.3, wall_buffer_m=0.85, wall_buffer_weight=100.0, - step_threshold_m=0.12, + step_threshold_m=0.25, step_penalty_weight=4.0, viz_publish_hz=PLANNER_VIZ_HZ, worker_threads=2, @@ -151,7 +151,8 @@ def _m20_rerun_blueprint() -> Any: ), DanLocalPlanner.blueprint( lock_replan=0.4, - resample_spacing_m=0.1, + # Preserve MLS's 3D waypoints; the 2D resampler replaces every Z with zero. + resample_spacing_m=0.0, ), DanHolonomicTC.blueprint( run_profile="walk", From 0541ac5b17ce81ee1e8f2e16489a0092a8689861 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 22:45:02 +0800 Subject: [PATCH 09/15] refactor(robot): simplify M20 lidar filtering --- .../m20/blueprints/m20_kronknav.py | 2 ++ .../deeprobotics/m20/pointlio/cpp/main.cpp | 36 ++----------------- .../robot/deeprobotics/m20/pointlio/module.py | 12 +++---- 3 files changed, 9 insertions(+), 41 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index 70dc1038f4..ba543697fe 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -129,6 +129,7 @@ def _m20_rerun_blueprint() -> Any: support_min=4, world_frame="odom", worker_threads=3, + cpu_affinity=frozenset({5}), ), MLSPlannerNative.blueprint( world_frame="odom", @@ -143,6 +144,7 @@ def _m20_rerun_blueprint() -> Any: step_penalty_weight=4.0, viz_publish_hz=PLANNER_VIZ_HZ, worker_threads=2, + cpu_affinity=frozenset({0, 1}), ).remappings( [ (MLSPlannerNative, "global_map", "global_map_unused"), diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp index e1dcc19288..be86b48c50 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp @@ -52,7 +52,6 @@ namespace { using Clock = std::chrono::steady_clock; constexpr double kStandardGravityMps2 = 9.80665; -constexpr std::size_t kPointLioStaticPointLimit = 100'000; constexpr std::size_t kM20RawPointLimit = 500'000; constexpr std::size_t kMaxInitializationLidarFrames = 20; @@ -179,7 +178,6 @@ struct M20PointLioConfig { double pointcloud_rate_hz; double odometry_rate_hz; double max_scan_duration_s; - int max_cloud_points; double msr_freq; double main_freq; bool con_frame; @@ -242,10 +240,6 @@ struct M20PointLioConfig { dimos::native::require_positive(max_scan_duration_s, "max_scan_duration_s"); dimos::native::require_positive(msr_freq, "msr_freq"); dimos::native::require_positive(main_freq, "main_freq"); - if (max_cloud_points <= 0 || - max_cloud_points > static_cast(kPointLioStaticPointLimit)) { - throw std::runtime_error("max_cloud_points must be in [1, 100000]"); - } if (scan_line <= 0 || scan_line > std::numeric_limits::max()) { throw std::runtime_error("scan_line must be in [1, 65535]"); } @@ -275,7 +269,6 @@ M20PointLioConfig parse_m20_pointlio_config(Config& config) { result.pointcloud_rate_hz = config.take("pointcloud_rate_hz"); result.odometry_rate_hz = config.take("odometry_rate_hz"); result.max_scan_duration_s = config.take("max_scan_duration_s"); - result.max_cloud_points = config.take("max_cloud_points"); result.msr_freq = config.take("msr_freq"); result.main_freq = config.take("main_freq"); result.con_frame = config.take("con_frame"); @@ -541,22 +534,12 @@ class M20PointLio : public Module { const auto point_count = static_cast(source.width) * static_cast(source.height); const auto point_step = static_cast(source.point_step); - const auto point_limit = static_cast(cfg_.max_cloud_points); - const auto source_sample_count = std::min(point_count, point_limit); std::vector points; - points.reserve(source_sample_count); + points.reserve(point_count); uint16_t min_ring = std::numeric_limits::max(); uint16_t max_ring = 0; - for (std::size_t sample_index = 0; sample_index < source_sample_count; - ++sample_index) { - const std::size_t index = - source_sample_count == point_count - ? sample_index - : (source_sample_count == 1 - ? point_count / 2 - : sample_index * (point_count - 1) / - (source_sample_count - 1)); + for (std::size_t index = 0; index < point_count; ++index) { const auto base = index * point_step; const float x = read_unaligned(source.data, base + offsets.x); const float y = read_unaligned(source.data, base + offsets.y); @@ -589,22 +572,10 @@ class M20PointLio : public Module { throw std::runtime_error("M20 cloud has no finite Point-LIO returns"); } - if (point_count > source_sample_count && - !logged_cloud_sampling_.exchange(true, std::memory_order_acq_rel)) { - logging::info( - "uniformly sampled M20 cloud before Point-LIO preprocessing", - {logging::Field("input_points", static_cast(point_count)), - logging::Field("sampled_source_points", - static_cast(source_sample_count)), - logging::Field("selected_valid_points", - static_cast(points.size()))}); - } - std::sort(points.begin(), points.end(), [](const TimedPoint& left, const TimedPoint& right) { return left.timestamp < right.timestamp; }); - const auto valid_point_count = points.size(); const double first_point_time = points.front().timestamp; const double last_point_time = points.back().timestamp; const double scan_duration = last_point_time - first_point_time; @@ -666,8 +637,6 @@ class M20PointLio : public Module { "M20 Point-LIO accepted cloud contract", {logging::Field("input_points", static_cast(point_count)), logging::Field("valid_points", - static_cast(valid_point_count)), - logging::Field("selected_points", static_cast(points.size())), logging::Field("min_ring", static_cast(min_ring)), logging::Field("max_ring", static_cast(max_ring)), @@ -825,7 +794,6 @@ class M20PointLio : public Module { bool lidar_feed_pending_ = false; std::atomic busy_lidar_drops_{0}; std::atomic logged_cloud_contract_{false}; - std::atomic logged_cloud_sampling_{false}; }; int main() { diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py index 3fa3bb5cd8..741e0de1fe 100644 --- a/dimos/robot/deeprobotics/m20/pointlio/module.py +++ b/dimos/robot/deeprobotics/m20/pointlio/module.py @@ -45,8 +45,8 @@ class M20PointLioConfig(NativeModuleConfig): } ) # GOS isolates its RK3588 big cores. Cores 6-7 run the vendor lidar - # drivers, so Point-LIO owns the otherwise-idle big cores 4-5. - cpu_affinity: frozenset[int] | None = frozenset({4, 5}) + # drivers, core 5 is assigned to mapping, and Point-LIO owns core 4. + cpu_affinity: frozenset[int] | None = frozenset({4}) lidar_topic: str = "/LIDAR/POINTS" imu_topic: str = "/IMU" @@ -56,10 +56,6 @@ class M20PointLioConfig(NativeModuleConfig): pointcloud_rate_hz: float = Field(default=10.0, gt=0.0) odometry_rate_hz: float = Field(default=50.0, gt=0.0) max_scan_duration_s: float = Field(default=0.2, gt=0.0) - # Live merged M20 frames contain roughly 100k returns. Point-LIO cannot - # process that rate in real time on the RK3588, so the native adapter - # uniformly selects this many returns before sorting and preprocessing. - max_cloud_points: int = Field(default=20_000, gt=0, le=100_000) msr_freq: float = Field(default=200.0, gt=0.0) main_freq: float = Field(default=1000.0, gt=0.0) @@ -74,7 +70,9 @@ class M20PointLioConfig(NativeModuleConfig): scan_line: int = Field(default=192, gt=0) scan_rate: int = Field(default=10, gt=0) blind: float = Field(default=0.5, ge=0.0) - point_filter_num: int = Field(default=3, gt=0) + # Use Point-LIO's standard pre-KF decimator for the merged dual-lidar cloud. + # Queue growth is bounded separately by the adapter's one-frame admission gate. + point_filter_num: int = Field(default=8, gt=0) use_imu_as_input: bool = False prop_at_freq_of_imu: bool = True From 515982ed978da02bb42455f592c9154180226c4c Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Sun, 30 Aug 2026 23:54:22 +0800 Subject: [PATCH 10/15] feat(robot): add M20 camera streams --- .../m20/blueprints/m20_kronknav.py | 124 +++++++++++++++++- dimos/robot/deeprobotics/m20/constants.py | 4 + 2 files changed, 121 insertions(+), 7 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index ba543697fe..a36754a749 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -14,11 +14,20 @@ """Robot-local M20 integration for the current DimOS 3D navigation stack.""" +import threading +import time from typing import Any +import av + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.coordination.blueprints import autoconnect +from dimos.core.core import rpc from dimos.core.global_config import global_config +from dimos.core.module import Module +from dimos.core.stream import Out from dimos.mapping.ray_tracing.module import RayTracingVoxelMap +from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager @@ -30,17 +39,95 @@ BASE_LINK_HEIGHT_M, BODY_LENGTH_M, BODY_WIDTH_M, + FRONT_CAMERA_RTSP_URL, MAX_ANGULAR_Z_RAD_S, MAX_LINEAR_X_M_S, MAX_LINEAR_Y_M_S, PLANNING_HEIGHT_M, + REAR_CAMERA_RTSP_URL, ROTATION_DIAMETER_M, ) from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLio +from dimos.utils.logging_config import setup_logger from dimos.visualization.vis_module import vis_module VOXEL_SIZE_M = 0.1 PLANNER_VIZ_HZ = 0.0 +logger = setup_logger() + + +class _M20CameraRelay(Module): + """Relay the vendor RTSP cameras as compressed H.265 DimOS streams.""" + + front_camera: Out[CompressedVideo] + rear_camera: Out[CompressedVideo] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._stop_event = threading.Event() + self._threads: list[threading.Thread] = [] + + @rpc + def start(self) -> None: + super().start() + self._stop_event.clear() + streams = ( + (FRONT_CAMERA_RTSP_URL, self.front_camera, "m20_front_camera"), + (REAR_CAMERA_RTSP_URL, self.rear_camera, "m20_rear_camera"), + ) + self._threads = [ + threading.Thread( + target=self._relay, + args=stream, + name=f"{stream[2]}-rtsp", + daemon=True, + ) + for stream in streams + ] + for thread in self._threads: + thread.start() + + def _relay(self, url: str, output: Out[CompressedVideo], camera_name: str) -> None: + while not self._stop_event.is_set(): + try: + with av.open( + url, + options={"rtsp_transport": "tcp", "fflags": "nobuffer"}, + timeout=(1.0, 1.0), + ) as container: + video = container.streams.video[0] + if video.codec_context.name != "hevc": + raise ValueError(f"expected H.265, got {video.codec_context.name}") + annex_b = av.BitStreamFilterContext("hevc_mp4toannexb", video) + logger.info("M20 camera stream connected", camera=camera_name) + for packet in container.demux(video): + if self._stop_event.is_set(): + return + for filtered in annex_b.filter(packet): + if filtered.size: + output.publish( + CompressedVideo( + bytes(filtered), + format="h265", + frame_id="", + ts=time.time(), + ) + ) + except (av.FFmpegError, IndexError, ValueError) as exc: + logger.warning( + "M20 camera stream unavailable", + camera=camera_name, + error=str(exc), + ) + self._stop_event.wait(2.0) + + @rpc + def stop(self) -> None: + self._stop_event.set() + for thread in self._threads: + thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self._threads.clear() + super().stop() def _render_global_map(msg: Any) -> Any: @@ -53,6 +140,16 @@ def _render_path(msg: Any) -> Any: return msg +def _render_camera_packet(msg: CompressedVideo) -> Any: + import rerun as rr + + return rr.VideoStream.from_fields(sample=msg.data.tobytes()) + + +def _static_h265_video(rr: Any) -> Any: + return rr.VideoStream(codec=rr.VideoCodec.H265) + + def _static_robot_body(rr: Any) -> list[Any]: return [ rr.Boxes3D( @@ -69,18 +166,26 @@ def _static_robot_body(rr: Any) -> list[Any]: def _m20_rerun_blueprint() -> Any: - """Go2-style navigation layout, adapted for the camera-less M20.""" + """Go2-style navigation layout with both M20 camera streams.""" import rerun as rr import rerun.blueprint as rrb return rrb.Blueprint( - rrb.Spatial3DView( - origin="world", - name="M20 KronkNav", - background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), - line_grid=rrb.LineGrid3D( - plane=rr.components.Plane3D.XY.with_distance(0.5), + rrb.Horizontal( + rrb.Vertical( + rrb.Spatial2DView(origin="world/front_camera", name="Front Camera"), + rrb.Spatial2DView(origin="world/rear_camera", name="Rear Camera"), + row_shares=[1, 1], + ), + rrb.Spatial3DView( + origin="world", + name="M20 KronkNav", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D( + plane=rr.components.Plane3D.XY.with_distance(0.5), + ), ), + column_shares=[1, 2], ), rrb.TimePanel(state="hidden"), rrb.SelectionPanel(state="hidden"), @@ -104,16 +209,21 @@ def _m20_rerun_blueprint() -> Any: "world/global_map": _render_global_map, "world/planner_path": None, "world/path": _render_path, + "world/front_camera": _render_camera_packet, + "world/rear_camera": _render_camera_packet, **planner_visual_override(PLANNER_VIZ_HZ), }, "static": { "world/robot_body": _static_robot_body, + "world/front_camera": _static_h265_video, + "world/rear_camera": _static_h265_video, }, } deeprobotics_m20_kronknav_control = autoconnect( vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + _M20CameraRelay.blueprint(instance_name="M20CameraRelay"), M20ROSBridge.blueprint( max_linear_x=MAX_LINEAR_X_M_S, max_linear_y=MAX_LINEAR_Y_M_S, diff --git a/dimos/robot/deeprobotics/m20/constants.py b/dimos/robot/deeprobotics/m20/constants.py index 036d55e221..85062463b0 100644 --- a/dimos/robot/deeprobotics/m20/constants.py +++ b/dimos/robot/deeprobotics/m20/constants.py @@ -36,3 +36,7 @@ MAX_LINEAR_X_M_S = 2.0 MAX_LINEAR_Y_M_S = 1.0 MAX_ANGULAR_Z_RAD_S = 2.0 + +# Vendor AOS H.265 camera streams documented for the M20 internal network. +FRONT_CAMERA_RTSP_URL = "rtsp://10.21.31.103:8554/video1" +REAR_CAMERA_RTSP_URL = "rtsp://10.21.31.103:8554/video2" From dee6b76d721f43b00783c1d10abfc05cf77d12ac Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 31 Aug 2026 16:48:28 +0800 Subject: [PATCH 11/15] refactor(robot): simplify M20 deployment setup --- dimos/robot/deeprobotics/m20/deploy/README.md | 91 +++++-------------- .../deploy/dimos-m20-fastdds-permissions.path | 13 --- .../dimos-m20-fastdds-permissions.service | 10 -- .../deploy/dimos-m20-rsdriver-shm-permissions | 49 +--------- .../10-dimos-command-ownership.conf | 6 -- .../10-dimos-shm-permissions.conf | 4 +- 6 files changed, 28 insertions(+), 145 deletions(-) delete mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path delete mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service delete mode 100644 dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index c82422beae..5133de23ae 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -1,102 +1,53 @@ # M20 deployment -The X20/M20 integration has one runnable blueprint: -`deeprobotics-m20-kronknav-control`. It starts command output disarmed. +Run `deeprobotics-m20-kronknav-control` on GOS (`10.21.31.104`). +Run the provisioning commands below from this `deploy` directory. -The sensor path stays on GOS and never sends raw lidar through LCM: +## One-time robot setup -```text -M20 lidar + IMU -- Fast DDS --> M20PointLio -- LCM --> mapping/navigation -M20 state/control -- Fast DDS <--> M20ROSBridge -- LCM <--> M20Connection -``` - -`/LIDAR/POINTS` is the vendor-merged `base_link` cloud. Its point records are -little-endian `float32 x/y/z/intensity`, `uint16 ring`, and `float64 timestamp` -at byte offsets 0, 4, 8, 12, 16, and 18 respectively. - -## Persistent robot setup - -On NOS (`10.21.31.106`), install the lidar relay supervisor and reserve -`/NAV_CMD` for DimOS: +On NOS (`10.21.31.106`), enable lidar forwarding and stop the vendor planner +that also publishes `/NAV_CMD`: ```bash -sudo install -D -o root -g root -m 0755 \ - dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor \ +sudo install -Dm755 dimos-m20-multicast-relay-supervisor \ /usr/local/libexec/dimos-m20-multicast-relay-supervisor -sudo install -D -o root -g root -m 0644 \ - dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf \ +sudo install -Dm644 multicast-relay.service.d/10-dimos-network-readiness.conf \ /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf -sudo install -D -o root -g root -m 0644 \ - dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf \ - /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf -sudo systemctl unmask planner.service sudo systemctl daemon-reload -sudo systemctl stop planner.service sudo systemctl enable --now multicast-relay.service +sudo systemctl disable --now planner.service ``` -On GOS (`10.21.31.104`), install the Fast DDS shared-memory permission hooks: +On GOS, let the normal `user` account read the root-owned Fast DDS objects +created by `rsdriver`: ```bash -sudo install -D -o root -g root -m 0755 \ - dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions \ +sudo install -Dm755 dimos-m20-rsdriver-shm-permissions \ /usr/local/libexec/dimos-m20-rsdriver-shm-permissions -sudo install -D -o root -g root -m 0644 \ - dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf \ +sudo install -Dm644 rsdriver.service.d/10-dimos-shm-permissions.conf \ /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf -sudo install -D -o root -g root -m 0644 \ - dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path \ - /etc/systemd/system/dimos-m20-fastdds-permissions.path -sudo install -D -o root -g root -m 0644 \ - dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service \ - /etc/systemd/system/dimos-m20-fastdds-permissions.service sudo systemctl daemon-reload -sudo systemctl enable --now rsdriver.service dimos-m20-fastdds-permissions.path +sudo systemctl enable --now rsdriver.service ``` -The `rsdriver` hook fixes existing Fast DDS files after driver start. The path -unit handles files created later. Keep both. - -## Run on GOS - -The native module declarations carry their own ROS library path, RMW selection, -and Fast DDS profile. No ROS setup script or environment file is required: +## Run ```bash cd /var/opt/robot/data/dimos-m20-kronknav source .venv/bin/activate -LCM_DEFAULT_URL='udpm://239.255.76.67:7667?ttl=0&recv_buf_size=16777216' \ - dimos --rerun-open none --rerun-host 0.0.0.0 \ - run deeprobotics-m20-kronknav-control +dimos --transport lcm --rerun-host 0.0.0.0 run deeprobotics-m20-kronknav-control --daemon ``` -Attach and enable control deliberately: +Connect the viewer: ```bash -dimos --transport lcm shell +dimos-viewer \ + --connect rerun+http://10.21.31.104:9877/proxy \ + --ws-url ws://10.21.31.104:3030/ws ``` -```python -app.M20Connection.standup() -``` - -`standup()` selects the navigation usage mode, enters RL Control, chooses gait -`0x3002`, and arms bounded velocity output. - -## Minimal checks +Attach the RPC shell: ```bash -# NOS -systemctl is-active multicast-relay.service -systemctl is-active planner.service # must be inactive - -# GOS -systemctl is-active rsdriver.service -systemctl is-active dimos-m20-fastdds-permissions.path -dimos status -dimos log +dimos --transport lcm shell ``` - -To restore the vendor planner instead of DimOS control, create -`/etc/dimos/enable-vendor-m20-planner` on NOS and start `planner.service`. Never -run both command owners at once. diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path deleted file mode 100644 index a536339e7f..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.path +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -[Unit] -Description=Watch M20 Fast DDS shared-memory objects -After=dev-shm.mount - -[Path] -PathChanged=/dev/shm -Unit=dimos-m20-fastdds-permissions.service - -[Install] -WantedBy=multi-user.target diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service deleted file mode 100644 index 3e62199e5f..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-fastdds-permissions.service +++ /dev/null @@ -1,10 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -[Unit] -Description=Expose M20 Fast DDS shared memory to the onboard DimOS user -After=dev-shm.mount - -[Service] -Type=oneshot -ExecStart=/usr/local/libexec/dimos-m20-rsdriver-shm-permissions --all diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions index 9b68ddb252..00a163f3a9 100755 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions +++ b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions @@ -4,48 +4,16 @@ set -euo pipefail -apply_fastdds_permissions() { - local path - local -a shm_files=() - - mapfile -d '' -t shm_files < <( - find /dev/shm -maxdepth 1 -type f \ - \( -name 'fastrtps_*' -o -name 'sem.fastrtps_*' \) -print0 - ) - - for path in "${shm_files[@]}"; do - [[ -e "$path" ]] || continue - chgrp user -- "$path" || { - [[ ! -e "$path" ]] || return 1 - continue - } - chmod g+rw -- "$path" || { - [[ ! -e "$path" ]] || return 1 - } - done -} - -if [[ "${1:-}" == "--all" ]]; then - # Fast DDS creates a segment, port, lock and semaphore in separate steps. - # Rescan briefly so a single systemd path activation covers the whole set. - for _attempt in $(seq 1 20); do - apply_fastdds_permissions - sleep 0.1 - done - exit 0 -fi - service_pid="${1:-}" if [[ ! "$service_pid" =~ ^[0-9]+$ ]] || [[ ! -d "/proc/$service_pid" ]]; then echo "expected the live rsdriver.service MainPID, got: $service_pid" >&2 exit 1 fi -settle_passes=0 for _attempt in $(seq 1 50); do driver_pid="$(pgrep --parent "$service_pid" --exact rslidar | head -n 1 || true)" if [[ -n "$driver_pid" ]]; then - mapfile -t driver_shm_files < <( + mapfile -t shm_files < <( { awk '$NF ~ /^\/dev\/shm\/fastrtps_/ {print $NF}' "/proc/$driver_pid/maps" for fd in "/proc/$driver_pid"/fd/*; do @@ -55,19 +23,12 @@ for _attempt in $(seq 1 50); do | grep -E '^/dev/shm/fastrtps_([0-9a-f]+|port[0-9]+)(_el)?$' \ | sort -u ) - if (( ${#driver_shm_files[@]} >= 4 )); then - ((settle_passes += 1)) + if (( ${#shm_files[@]} >= 4 )); then + chgrp user -- "${shm_files[@]}" + chmod g+rw -- "${shm_files[@]}" + exit 0 fi fi - - # A Foxy reader scans every same-host Fast DDS participant before matching - # rslidar. Root-owned encryption/cpu segments can therefore block the cloud - # even when rslidar's own files are writable. - apply_fastdds_permissions - - if (( settle_passes >= 20 )); then - exit 0 - fi sleep 0.1 done diff --git a/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf b/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf deleted file mode 100644 index 9afb99a887..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/planner.service.d/10-dimos-command-ownership.conf +++ /dev/null @@ -1,6 +0,0 @@ -# DimOS owns /NAV_CMD on M20 integrations. The vendor manual requires its -# planner service to be stopped before an external publisher uses that topic. -# Create /etc/dimos/enable-vendor-m20-planner to deliberately restore the -# vendor planner instead of allowing two command owners by accident. -[Unit] -ConditionPathExists=/etc/dimos/enable-vendor-m20-planner diff --git a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf index a82d6144ac..dff520a1d7 100644 --- a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf +++ b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf @@ -1,6 +1,6 @@ # GOS runs rslidar as root so it can request real-time scheduling and publish a -# usable Fast DDS writer on this vendor image. Keep root execution, but let the -# normal onboard `user` account attach to the local DDS shared-memory objects. +# usable Fast DDS writer on this vendor image. Keep root execution, but expose +# only the driver's shared-memory objects to the normal onboard `user` account. [Service] Group=user UMask=0002 From 029ef619a44330e576917b98728bb8141e47b8fc Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Mon, 31 Aug 2026 17:35:44 +0800 Subject: [PATCH 12/15] chore(robot): automate M20 provisioning --- dimos/robot/deeprobotics/m20/deploy/README.md | 40 ++++++-------- dimos/robot/deeprobotics/m20/deploy/setup.sh | 54 +++++++++++++++++++ 2 files changed, 71 insertions(+), 23 deletions(-) create mode 100755 dimos/robot/deeprobotics/m20/deploy/setup.sh diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index 5133de23ae..a2353abf27 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -1,41 +1,28 @@ # M20 deployment -Run `deeprobotics-m20-kronknav-control` on GOS (`10.21.31.104`). -Run the provisioning commands below from this `deploy` directory. +Run `deeprobotics-m20-kronknav-control` on the M20 development computer +(`10.21.31.104`). ## One-time robot setup -On NOS (`10.21.31.106`), enable lidar forwarding and stop the vendor planner -that also publishes `/NAV_CMD`: +From the repository checkout on `10.21.31.104`, run: ```bash -sudo install -Dm755 dimos-m20-multicast-relay-supervisor \ - /usr/local/libexec/dimos-m20-multicast-relay-supervisor -sudo install -Dm644 multicast-relay.service.d/10-dimos-network-readiness.conf \ - /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf -sudo systemctl daemon-reload -sudo systemctl enable --now multicast-relay.service -sudo systemctl disable --now planner.service +./dimos/robot/deeprobotics/m20/deploy/setup.sh ``` -On GOS, let the normal `user` account read the root-owned Fast DDS objects -created by `rsdriver`: - -```bash -sudo install -Dm755 dimos-m20-rsdriver-shm-permissions \ - /usr/local/libexec/dimos-m20-rsdriver-shm-permissions -sudo install -Dm644 rsdriver.service.d/10-dimos-shm-permissions.conf \ - /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf -sudo systemctl daemon-reload -sudo systemctl enable --now rsdriver.service -``` +The script configures the other onboard computer to forward lidar data and stop +its competing vendor planner, then configures this computer to expose the lidar +data to DimOS. It is safe to run again and persists across reboots. ## Run ```bash cd /var/opt/robot/data/dimos-m20-kronknav +uv sync --extra all source .venv/bin/activate -dimos --transport lcm --rerun-host 0.0.0.0 run deeprobotics-m20-kronknav-control --daemon +dimos --build-native --transport lcm --rerun-host 0.0.0.0 \ + run deeprobotics-m20-kronknav-control --daemon ``` Connect the viewer: @@ -51,3 +38,10 @@ Attach the RPC shell: ```bash dimos --transport lcm shell ``` + +Then stand the robot up or lie it down: + +```python +app.M20Connection.standup() +app.M20Connection.liedown() +``` diff --git a/dimos/robot/deeprobotics/m20/deploy/setup.sh b/dimos/robot/deeprobotics/m20/deploy/setup.sh new file mode 100755 index 0000000000..b81937c4c0 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/setup.sh @@ -0,0 +1,54 @@ +#!/usr/bin/env bash +# Copyright 2026 Dimensional Inc. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +deploy_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +navigation_host="${M20_NAVIGATION_HOST:-user@10.21.31.106}" + +if ! systemctl cat rsdriver.service >/dev/null 2>&1; then + echo "Run this script on the M20 computer at 10.21.31.104." >&2 + exit 1 +fi + +echo "Configuring lidar forwarding on ${navigation_host}..." +tar -C "$deploy_dir" -cf - \ + dimos-m20-multicast-relay-supervisor \ + multicast-relay.service.d/10-dimos-network-readiness.conf | \ + ssh -o StrictHostKeyChecking=accept-new -o ConnectTimeout=10 "$navigation_host" ' + set -eu + setup_dir=$(mktemp -d /tmp/dimos-m20-setup.XXXXXX) + trap '\''rm -f "$setup_dir/dimos-m20-multicast-relay-supervisor" "$setup_dir/multicast-relay.service.d/10-dimos-network-readiness.conf"; rmdir "$setup_dir/multicast-relay.service.d" "$setup_dir" 2>/dev/null || true'\'' EXIT + tar -xf - -C "$setup_dir" + sudo install -Dm755 "$setup_dir/dimos-m20-multicast-relay-supervisor" /usr/local/libexec/dimos-m20-multicast-relay-supervisor + sudo install -Dm644 "$setup_dir/multicast-relay.service.d/10-dimos-network-readiness.conf" /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf + sudo rm -f /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf + sudo systemctl daemon-reload + sudo systemctl enable multicast-relay.service + sudo systemctl restart multicast-relay.service + sudo systemctl disable --now planner.service + systemctl is-active --quiet multicast-relay.service + ! systemctl is-active --quiet planner.service + ' + +echo "Configuring lidar access on 10.21.31.104..." +sudo install -Dm755 \ + "$deploy_dir/dimos-m20-rsdriver-shm-permissions" \ + /usr/local/libexec/dimos-m20-rsdriver-shm-permissions +sudo install -Dm644 \ + "$deploy_dir/rsdriver.service.d/10-dimos-shm-permissions.conf" \ + /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf + +if sudo test -e /etc/systemd/system/dimos-m20-fastdds-permissions.path; then + sudo systemctl disable --now dimos-m20-fastdds-permissions.path +fi +sudo rm -f \ + /etc/systemd/system/dimos-m20-fastdds-permissions.path \ + /etc/systemd/system/dimos-m20-fastdds-permissions.service +sudo systemctl daemon-reload +sudo systemctl enable rsdriver.service +sudo systemctl restart rsdriver.service +systemctl is-active --quiet rsdriver.service + +echo "M20 setup complete." From 527654124aa354500d164c4b4c50c5e540f04e68 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 2 Sep 2026 03:21:02 +0800 Subject: [PATCH 13/15] refactor(robot): simplify M20 onboard integration --- dimos/core/native_module.py | 19 +- dimos/robot/all_blueprints.py | 2 - .../m20/blueprints/m20_kronknav.py | 254 ++---- .../m20/bridge/cpp/CMakeLists.txt | 52 -- .../deeprobotics/m20/bridge/cpp/build.sh | 41 - .../deeprobotics/m20/bridge/cpp/main.cpp | 415 --------- dimos/robot/deeprobotics/m20/bridge/module.py | 88 -- dimos/robot/deeprobotics/m20/camera.py | 191 +++++ dimos/robot/deeprobotics/m20/connection.py | 8 + dimos/robot/deeprobotics/m20/constants.py | 7 - dimos/robot/deeprobotics/m20/deploy/README.md | 37 +- .../dimos-m20-multicast-relay-supervisor | 95 --- .../deploy/dimos-m20-rsdriver-shm-permissions | 36 - .../m20/deploy/drdds-zenoh-bridge.service | 19 + .../localization.service.d/10-dimos-lio.conf | 17 + .../10-dimos-network-readiness.conf | 11 - .../10-dimos-shm-permissions.conf | 7 - dimos/robot/deeprobotics/m20/deploy/setup.sh | 110 ++- .../drdds-zenoh-bridge/cpp/CMakeLists.txt | 51 ++ .../onboard/drdds-zenoh-bridge/cpp/build.sh | 20 + .../onboard/drdds-zenoh-bridge/cpp/main.cpp | 714 ++++++++++++++++ .../m20/pointlio/cpp/CMakeLists.txt | 112 --- .../deeprobotics/m20/pointlio/cpp/build.sh | 44 - .../deeprobotics/m20/pointlio/cpp/main.cpp | 802 ------------------ .../m20/pointlio/cpp/pointlio-gos.patch | 16 - .../robot/deeprobotics/m20/pointlio/module.py | 141 --- native/cpp/include/dimos/native/config.hpp | 34 +- 27 files changed, 1218 insertions(+), 2125 deletions(-) delete mode 100644 dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt delete mode 100755 dimos/robot/deeprobotics/m20/bridge/cpp/build.sh delete mode 100644 dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp delete mode 100644 dimos/robot/deeprobotics/m20/bridge/module.py create mode 100644 dimos/robot/deeprobotics/m20/camera.py delete mode 100644 dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor delete mode 100755 dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions create mode 100644 dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service create mode 100644 dimos/robot/deeprobotics/m20/deploy/localization.service.d/10-dimos-lio.conf delete mode 100644 dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf delete mode 100644 dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf create mode 100644 dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/CMakeLists.txt create mode 100755 dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/build.sh create mode 100644 dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt delete mode 100755 dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch delete mode 100644 dimos/robot/deeprobotics/m20/pointlio/module.py diff --git a/dimos/core/native_module.py b/dimos/core/native_module.py index 68ede8a293..84fea6b685 100644 --- a/dimos/core/native_module.py +++ b/dimos/core/native_module.py @@ -75,14 +75,8 @@ def _set_process_to_die_when_parent_dies() -> None: if _LIBC.prctl(_PR_SET_PDEATHSIG, signal.SIGTERM) != 0: err = ctypes.get_errno() raise OSError(err, f"_set_process_to_die_when_parent_dies failed: {os.strerror(err)}") - - def _configure_native_child(cpu_affinity: frozenset[int] | None) -> None: - _set_process_to_die_when_parent_dies() - if cpu_affinity is not None: - os.sched_setaffinity(0, cpu_affinity) else: _set_process_to_die_when_parent_dies = None # type: ignore[assignment] - _configure_native_child = None # type: ignore[assignment] if sys.version_info < (3, 13): from typing_extensions import TypeVar @@ -128,8 +122,6 @@ class NativeModuleConfig(ModuleConfig): cwd: str | None = None extra_args: list[str] = Field(default_factory=list) extra_env: dict[str, str] = Field(default_factory=dict) - # Optional Linux CPU set inherited by every thread in the native child. - cpu_affinity: frozenset[int] | None = None # Session settings for this module alone, e.g. opening it as the zenoh router # the rest of the graph connects to. None follows the global config. session: SessionConfig | None = None @@ -314,15 +306,6 @@ def start(self) -> None: module=self._module_label, cmd=" ".join(cmd), cwd=cwd, - cpu_affinity=( - sorted(self.config.cpu_affinity) if self.config.cpu_affinity is not None else None - ), - ) - - child_setup = ( - functools.partial(_configure_native_child, self.config.cpu_affinity) - if _configure_native_child is not None - else None ) self._process = subprocess.Popen( @@ -333,7 +316,7 @@ def start(self) -> None: stdout=subprocess.PIPE, stderr=subprocess.PIPE, start_new_session=True, - preexec_fn=child_setup, + preexec_fn=_set_process_to_die_when_parent_dies, ) assert self._process.stdin is not None if stdin_blob is not None: diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 14ac1e5bec..a9e608fdf3 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -228,8 +228,6 @@ "keyboard-teleop": "dimos.robot.unitree.keyboard_teleop.KeyboardTeleop", "keyboard-teleop-module": "dimos.teleop.keyboard.keyboard_teleop_module.KeyboardTeleopModule", "m20-connection": "dimos.robot.deeprobotics.m20.connection.M20Connection", - "m20-point-lio": "dimos.robot.deeprobotics.m20.pointlio.module.M20PointLio", - "m20-ros-bridge": "dimos.robot.deeprobotics.m20.bridge.module.M20ROSBridge", "manipulation-module": "dimos.manipulation.manipulation_module.ManipulationModule", "manipulation-skills": "dimos.manipulation.manipulation_skills.ManipulationSkills", "map": "dimos.robot.unitree.type.map.Map", diff --git a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py index a36754a749..163e41f6c5 100644 --- a/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py +++ b/dimos/robot/deeprobotics/m20/blueprints/m20_kronknav.py @@ -14,124 +14,33 @@ """Robot-local M20 integration for the current DimOS 3D navigation stack.""" -import threading -import time from typing import Any -import av - -from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.coordination.blueprints import autoconnect -from dimos.core.core import rpc from dimos.core.global_config import global_config -from dimos.core.module import Module -from dimos.core.stream import Out +from dimos.core.transport import ZenohTransport from dimos.mapping.ray_tracing.module import RayTracingVoxelMap from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.navigation.dannav.holonomic_tc.module import DanHolonomicTC from dimos.navigation.dannav.local_planner.module import DanLocalPlanner from dimos.navigation.movement_manager.movement_manager import MovementManager from dimos.navigation.nav_3d.mls_planner.mls_planner_native import MLSPlannerNative from dimos.navigation.nav_3d.mls_planner.viz import planner_visual_override -from dimos.robot.deeprobotics.m20.bridge.module import M20ROSBridge +from dimos.protocol.pubsub.impl.zenohpubsub import QOS_LATEST_WINS, Topic as ZenohTopic +from dimos.robot.deeprobotics.m20.camera import M20CameraRelay from dimos.robot.deeprobotics.m20.connection import M20Connection from dimos.robot.deeprobotics.m20.constants import ( BASE_LINK_HEIGHT_M, BODY_LENGTH_M, BODY_WIDTH_M, - FRONT_CAMERA_RTSP_URL, - MAX_ANGULAR_Z_RAD_S, - MAX_LINEAR_X_M_S, - MAX_LINEAR_Y_M_S, PLANNING_HEIGHT_M, - REAR_CAMERA_RTSP_URL, ROTATION_DIAMETER_M, ) -from dimos.robot.deeprobotics.m20.pointlio.module import M20PointLio -from dimos.utils.logging_config import setup_logger from dimos.visualization.vis_module import vis_module VOXEL_SIZE_M = 0.1 PLANNER_VIZ_HZ = 0.0 -logger = setup_logger() - - -class _M20CameraRelay(Module): - """Relay the vendor RTSP cameras as compressed H.265 DimOS streams.""" - - front_camera: Out[CompressedVideo] - rear_camera: Out[CompressedVideo] - - def __init__(self, **kwargs: Any) -> None: - super().__init__(**kwargs) - self._stop_event = threading.Event() - self._threads: list[threading.Thread] = [] - - @rpc - def start(self) -> None: - super().start() - self._stop_event.clear() - streams = ( - (FRONT_CAMERA_RTSP_URL, self.front_camera, "m20_front_camera"), - (REAR_CAMERA_RTSP_URL, self.rear_camera, "m20_rear_camera"), - ) - self._threads = [ - threading.Thread( - target=self._relay, - args=stream, - name=f"{stream[2]}-rtsp", - daemon=True, - ) - for stream in streams - ] - for thread in self._threads: - thread.start() - - def _relay(self, url: str, output: Out[CompressedVideo], camera_name: str) -> None: - while not self._stop_event.is_set(): - try: - with av.open( - url, - options={"rtsp_transport": "tcp", "fflags": "nobuffer"}, - timeout=(1.0, 1.0), - ) as container: - video = container.streams.video[0] - if video.codec_context.name != "hevc": - raise ValueError(f"expected H.265, got {video.codec_context.name}") - annex_b = av.BitStreamFilterContext("hevc_mp4toannexb", video) - logger.info("M20 camera stream connected", camera=camera_name) - for packet in container.demux(video): - if self._stop_event.is_set(): - return - for filtered in annex_b.filter(packet): - if filtered.size: - output.publish( - CompressedVideo( - bytes(filtered), - format="h265", - frame_id="", - ts=time.time(), - ) - ) - except (av.FFmpegError, IndexError, ValueError) as exc: - logger.warning( - "M20 camera stream unavailable", - camera=camera_name, - error=str(exc), - ) - self._stop_event.wait(2.0) - - @rpc - def stop(self) -> None: - self._stop_event.set() - for thread in self._threads: - thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - self._threads.clear() - super().stop() - - -def _render_global_map(msg: Any) -> Any: - return msg.to_rerun() def _render_path(msg: Any) -> Any: @@ -140,14 +49,24 @@ def _render_path(msg: Any) -> Any: return msg -def _render_camera_packet(msg: CompressedVideo) -> Any: +def _render_h265(msg: CompressedVideo) -> Any: import rerun as rr - return rr.VideoStream.from_fields(sample=msg.data.tobytes()) + return rr.VideoStream(codec=rr.VideoCodec.H265, sample=msg.data.tobytes()) + + +def _render_front_camera_info(msg: CameraInfo) -> Any: + return msg.to_rerun( + image_topic="world/front_camera", + optical_frame="front_camera_optical", + ) -def _static_h265_video(rr: Any) -> Any: - return rr.VideoStream(codec=rr.VideoCodec.H265) +def _render_rear_camera_info(msg: CameraInfo) -> Any: + return msg.to_rerun( + image_topic="world/rear_camera", + optical_frame="rear_camera_optical", + ) def _static_robot_body(rr: Any) -> list[Any]: @@ -194,87 +113,88 @@ def _m20_rerun_blueprint() -> Any: _rerun_config = { "blueprint": _m20_rerun_blueprint, - # Match the Go2 navigation replay budget so a newly attached viewer catches - # up quickly instead of replaying a large sensor backlog. - "memory_limit": "64MB", "tf_axes": 0.35, "max_hz": { "world/local_map": 0.5, - # RayTracingVoxelMap already limits this at the source. - "world/global_map": 0, }, "visual_override": { # The navigation view shows maps rather than the registered lidar. "world/lidar": None, - "world/global_map": _render_global_map, - "world/planner_path": None, - "world/path": _render_path, - "world/front_camera": _render_camera_packet, - "world/rear_camera": _render_camera_packet, + "world/slam_body_points": None, + "world/planner_path": _render_path, + "world/path": None, + "world/front_camera": _render_h265, + "world/rear_camera": _render_h265, + "world/front_camera_info": _render_front_camera_info, + "world/rear_camera_info": _render_rear_camera_info, **planner_visual_override(PLANNER_VIZ_HZ), }, "static": { "world/robot_body": _static_robot_body, - "world/front_camera": _static_h265_video, - "world/rear_camera": _static_h265_video, }, } -deeprobotics_m20_kronknav_control = autoconnect( - vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), - _M20CameraRelay.blueprint(instance_name="M20CameraRelay"), - M20ROSBridge.blueprint( - max_linear_x=MAX_LINEAR_X_M_S, - max_linear_y=MAX_LINEAR_Y_M_S, - max_angular_z=MAX_ANGULAR_Z_RAD_S, - ), - M20PointLio.blueprint(), - M20Connection.blueprint(), - RayTracingVoxelMap.blueprint( - voxel_size=VOXEL_SIZE_M, - max_range=25.0, - emit_every=1, - global_emit_every=50, - support_min=4, - world_frame="odom", - worker_threads=3, - cpu_affinity=frozenset({5}), - ), - MLSPlannerNative.blueprint( - world_frame="odom", - base_frame="base_link", - voxel_size=VOXEL_SIZE_M, - robot_height=PLANNING_HEIGHT_M, - start_z_offset_m=BASE_LINK_HEIGHT_M, - wall_clearance_m=0.3, - wall_buffer_m=0.85, - wall_buffer_weight=100.0, - step_threshold_m=0.25, - step_penalty_weight=4.0, - viz_publish_hz=PLANNER_VIZ_HZ, - worker_threads=2, - cpu_affinity=frozenset({0, 1}), - ).remappings( - [ - (MLSPlannerNative, "global_map", "global_map_unused"), - (MLSPlannerNative, "path", "planner_path"), - ] - ), - DanLocalPlanner.blueprint( - lock_replan=0.4, - # Preserve MLS's 3D waypoints; the 2D resampler replaces every Z with zero. - resample_spacing_m=0.0, - ), - DanHolonomicTC.blueprint( - run_profile="walk", - control_frequency=10.0, - ), - MovementManager.blueprint(), -).global_config( - n_workers=4, - obstacle_avoidance=False, - robot_width=BODY_WIDTH_M, - robot_rotation_diameter=ROTATION_DIAMETER_M, - transport="lcm", +deeprobotics_m20_kronknav_control = ( + autoconnect( + vis_module(viewer_backend=global_config.viewer, rerun_config=_rerun_config), + M20CameraRelay.blueprint(instance_name="M20CameraRelay"), + M20Connection.blueprint().remappings([(M20Connection, "odometry", "slam_odom")]), + RayTracingVoxelMap.blueprint( + voxel_size=VOXEL_SIZE_M, + max_range=25.0, + ray_subsample=5, + emit_every=1, + global_emit_every=50, + support_min=4, + world_frame="map", + worker_threads=3, + ).remappings([(RayTracingVoxelMap, "lidar", "slam_body_points")]), + MLSPlannerNative.blueprint( + world_frame="map", + base_frame="base_link", + voxel_size=VOXEL_SIZE_M, + robot_height=PLANNING_HEIGHT_M, + start_z_offset_m=BASE_LINK_HEIGHT_M, + wall_clearance_m=0.3, + wall_buffer_m=0.85, + wall_buffer_weight=100.0, + step_threshold_m=0.25, + step_penalty_weight=4.0, + viz_publish_hz=PLANNER_VIZ_HZ, + worker_threads=2, + ).remappings( + [ + (MLSPlannerNative, "global_map", "global_map_unused"), + (MLSPlannerNative, "path", "planner_path"), + ] + ), + DanLocalPlanner.blueprint( + lock_replan=0.4, + # Preserve MLS's 3D waypoints; the 2D resampler replaces every Z with zero. + resample_spacing_m=0.0, + ), + DanHolonomicTC.blueprint( + run_profile="walk", + control_frequency=10.0, + ), + MovementManager.blueprint(), + ) + .global_config( + n_workers=4, + obstacle_avoidance=False, + robot_width=BODY_WIDTH_M, + robot_rotation_diameter=ROTATION_DIAMETER_M, + transport="zenoh", + ) + .transports( + { + ("front_camera", CompressedVideo): ZenohTransport.spec( + ZenohTopic("dimos/front_camera", CompressedVideo, qos=QOS_LATEST_WINS) + ), + ("rear_camera", CompressedVideo): ZenohTransport.spec( + ZenohTopic("dimos/rear_camera", CompressedVideo, qos=QOS_LATEST_WINS) + ), + } + ) ) diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt deleted file mode 100644 index ad8769059c..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -cmake_minimum_required(VERSION 3.14) -project(m20_ros_bridge CXX) - -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -find_package(ament_cmake REQUIRED) -find_package(drdds REQUIRED) -find_package(rclcpp REQUIRED) -find_package(PkgConfig REQUIRED) -pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) - -if(DEFINED DIMOS_LCM_DIR) - set(dimos_lcm_SOURCE_DIR ${DIMOS_LCM_DIR}) -else() - include(FetchContent) - FetchContent_Declare(dimos_lcm - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git - GIT_TAG 0a1c24710ce2f7a569e1673617102cc5254a75e2 - ) - # dimos-lcm is a generated-header repository without a CMakeLists.txt, so it - # can be populated but not added as a CMake subproject. - FetchContent_GetProperties(dimos_lcm) - if(NOT dimos_lcm_POPULATED) - FetchContent_Populate(dimos_lcm) - endif() -endif() - -if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) - set(DIMOS_NATIVE_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../native/cpp) -endif() -add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) - -add_executable(m20_ros_bridge main.cpp) -target_include_directories(m20_ros_bridge PRIVATE - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs -) -ament_target_dependencies(m20_ros_bridge - drdds - rclcpp -) -target_link_libraries(m20_ros_bridge - dimos_native - PkgConfig::LCM -) -target_compile_options(m20_ros_bridge PRIVATE -Wall -Wextra -Wpedantic) - -install(TARGETS m20_ros_bridge DESTINATION bin) diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh b/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh deleted file mode 100755 index 6992060837..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/build.sh +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env bash -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -set -euo pipefail - -bridge_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -ros_setup="${M20_ROS_SETUP:-/opt/robot/scripts/setup_ros2.sh}" - -if [[ -f "$ros_setup" ]]; then - # shellcheck disable=SC1090 - set +u - source "$ros_setup" - set -u -elif [[ -f /opt/ros/foxy/setup.bash ]]; then - # shellcheck disable=SC1091 - set +u - source /opt/ros/foxy/setup.bash - set -u -else - echo "M20 ROS setup not found: $ros_setup" >&2 - exit 1 -fi - -cmake_args=( - -S "$bridge_dir" - -B "$bridge_dir/build" - -DCMAKE_BUILD_TYPE=Release -) -if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then - cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") -fi -if [[ -n "${M20_PFR_DIR:-}" ]]; then - cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_PFR=${M20_PFR_DIR}") -fi -if [[ -n "${M20_NLOHMANN_JSON_DIR:-}" ]]; then - cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_NLOHMANN_JSON=${M20_NLOHMANN_JSON_DIR}") -fi - -cmake "${cmake_args[@]}" -cmake --build "$bridge_dir/build" --parallel "${M20_BUILD_JOBS:-4}" diff --git a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp deleted file mode 100644 index dfc0e84147..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/cpp/main.cpp +++ /dev/null @@ -1,415 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// Robot-local command/state ROS 2/DrDDS adapter for the Deep Robotics Lynx M20. -// High-bandwidth lidar and IMU ingress belongs to the M20 Point-LIO process. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "dimos/native.hpp" - -#include "geometry_msgs/Twist.hpp" -#include "std_msgs/Bool.hpp" -#include "std_msgs/Int32.hpp" -#include "std_msgs/UInt32.hpp" - -using dimos::native::Builder; -using dimos::native::Config; -using dimos::native::Module; -using dimos::native::Output; -namespace logging = dimos::native::log; - -namespace { - -using Clock = std::chrono::steady_clock; -constexpr int64_t kNanosecondsPerSecond = 1'000'000'000LL; -constexpr int kMotionRlControl = 17; - -void require_nonempty(const std::string& value, const char* name) { - if (value.empty()) { - throw std::runtime_error(std::string(name) + " must not be empty"); - } -} - -double clamp(double value, double limit) { - return std::max(-limit, std::min(limit, value)); -} - -geometry_msgs::Twist zero_twist() { - geometry_msgs::Twist result; - result.linear.x = 0.0; - result.linear.y = 0.0; - result.linear.z = 0.0; - result.angular.x = 0.0; - result.angular.y = 0.0; - result.angular.z = 0.0; - return result; -} - -template -struct HasSec : std::false_type {}; -template -struct HasSec().sec)>> : std::true_type {}; - -template -struct HasNanosec : std::false_type {}; -template -struct HasNanosec().nanosec)>> : std::true_type {}; - -template -struct HasNsec : std::false_type {}; -template -struct HasNsec().nsec)>> : std::true_type {}; - -template -struct HasFrameId : std::false_type {}; -template -struct HasFrameId().frame_id)>> : std::true_type {}; - -template -struct HasTimestamp : std::false_type {}; -template -struct HasTimestamp().timestamp)>> - : std::true_type {}; - -template -struct HasStamp : std::false_type {}; -template -struct HasStamp().stamp)>> : std::true_type {}; - -template -struct HasValue : std::false_type {}; -template -struct HasValue().value)>> : std::true_type {}; - -template -struct HasData : std::false_type {}; -template -struct HasData().data)>> : std::true_type {}; - -template -void set_vendor_stamp(Stamp& stamp, int32_t sec, uint32_t nsec) { - static_assert(HasSec::value, "M20 vendor Timestamp must expose sec"); - static_assert(HasNanosec::value || HasNsec::value, - "M20 vendor Timestamp must expose nanosec or nsec"); - stamp.sec = sec; - if constexpr (HasNanosec::value) { - stamp.nanosec = nsec; - } else { - stamp.nsec = nsec; - } -} - -// Released M20 message packages have used both `stamp` and `timestamp` in -// MetaType. Keep the bridge source-compatible with either installed version. -template -void set_vendor_header(Header& header, uint64_t frame_id, const rclcpp::Time& now) { - static_assert(HasFrameId
::value, "M20 vendor MetaType must expose frame_id"); - static_assert(HasTimestamp
::value || HasStamp
::value, - "M20 vendor MetaType must expose timestamp or stamp"); - header.frame_id = frame_id; - const int64_t total_ns = now.nanoseconds(); - const auto sec = static_cast(total_ns / kNanosecondsPerSecond); - const auto nsec = static_cast(total_ns % kNanosecondsPerSecond); - if constexpr (HasTimestamp
::value) { - set_vendor_stamp(header.timestamp, sec, nsec); - } else { - set_vendor_stamp(header.stamp, sec, nsec); - } -} - -template -int vendor_int32_value(const Status& status) { - static_assert(HasValue::value || HasData::value, - "M20 vendor int32 status must expose value or data"); - if constexpr (HasValue::value) { - return static_cast(status.value); - } else { - return static_cast(status.data); - } -} - -} // namespace - -struct M20ROSBridgeConfig { - std::string nav_cmd_topic; - std::string motion_state_topic; - std::string motion_info_topic; - std::string gait_topic; - std::string hes_status_topic; - std::string node_name; - double command_rate_hz; - double command_timeout_s; - double safety_timeout_s; - double max_linear_x; - double max_linear_y; - double max_angular_z; - - void validate() const { - require_nonempty(nav_cmd_topic, "nav_cmd_topic"); - require_nonempty(motion_state_topic, "motion_state_topic"); - require_nonempty(motion_info_topic, "motion_info_topic"); - require_nonempty(gait_topic, "gait_topic"); - require_nonempty(hes_status_topic, "hes_status_topic"); - require_nonempty(node_name, "node_name"); - dimos::native::require_positive(command_rate_hz, "command_rate_hz"); - dimos::native::require_positive(command_timeout_s, "command_timeout_s"); - dimos::native::require_positive(safety_timeout_s, "safety_timeout_s"); - dimos::native::require_positive(max_linear_x, "max_linear_x"); - dimos::native::require_positive(max_linear_y, "max_linear_y"); - dimos::native::require_positive(max_angular_z, "max_angular_z"); - } -}; - -M20ROSBridgeConfig parse_m20_config(Config& config) { - M20ROSBridgeConfig result{}; - result.nav_cmd_topic = config.take("nav_cmd_topic"); - result.motion_state_topic = config.take("motion_state_topic"); - result.motion_info_topic = config.take("motion_info_topic"); - result.gait_topic = config.take("gait_topic"); - result.hes_status_topic = config.take("hes_status_topic"); - result.node_name = config.take("node_name"); - result.command_rate_hz = config.take("command_rate_hz"); - result.command_timeout_s = config.take("command_timeout_s"); - result.safety_timeout_s = config.take("safety_timeout_s"); - result.max_linear_x = config.take("max_linear_x"); - result.max_linear_y = config.take("max_linear_y"); - result.max_angular_z = config.take("max_angular_z"); - config.enforce_all_consumed(); - result.validate(); - return result; -} - -class M20ROSBridge : public Module { -public: - void build(Builder& builder, Config& config) override { - cfg_ = parse_m20_config(config); - builder.input("safe_cmd_vel", &M20ROSBridge::on_command, this); - builder.input("motion_state_cmd", - &M20ROSBridge::on_motion_state_command, this); - builder.input("gait_cmd", &M20ROSBridge::on_gait_command, this); - command_ready_ = builder.output("command_ready"); - motion_state_ = builder.output("motion_state"); - gait_state_ = builder.output("gait_state"); - } - - void setup() override { - rclcpp::init(0, nullptr); - // rclcpp installs process signal handlers during init. Restore the - // NativeModule handlers so coordinator SIGTERM exits Module::handle(); - // teardown below then cancels the executor and shuts rclcpp down. - dimos::native::install_signal_handlers(); - node_ = std::make_shared(cfg_.node_name); - - // The advertised M20 endpoint is RELIABLE/TRANSIENT_LOCAL. Some M20 - // firmware revisions expose the endpoint without actually emitting - // its documented 1 Hz samples, so HES is a veto when observed rather - // than the command-path heartbeat. The physical stop remains enforced - // below this API by the robot controller. - const auto hes_qos = - rclcpp::QoS(rclcpp::KeepLast(2)).reliable().transient_local(); - hes_subscription_ = node_->create_subscription( - cfg_.hes_status_topic, hes_qos, - [this](drdds::msg::StdMsgInt32::SharedPtr msg) { on_hes_status(*msg); }); - motion_info_subscription_ = node_->create_subscription( - cfg_.motion_info_topic, - rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(), - [this](drdds::msg::MotionInfo::SharedPtr msg) { on_motion_info(*msg); }); - - nav_cmd_publisher_ = node_->create_publisher( - cfg_.nav_cmd_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - motion_state_publisher_ = node_->create_publisher( - cfg_.motion_state_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - gait_publisher_ = node_->create_publisher( - cfg_.gait_topic, rclcpp::QoS(rclcpp::KeepLast(2)).reliable()); - - const auto period = std::chrono::duration(1.0 / cfg_.command_rate_hz); - timer_ = node_->create_wall_timer( - std::chrono::duration_cast(period), - [this]() { publish_cycle(false); }); - - executor_ = std::make_shared(); - executor_->add_node(node_); - spin_thread_ = std::thread([this]() { executor_->spin(); }); - - logging::info("M20 command/state ROS bridge started"); - } - - void teardown() override { - stopping_.store(true, std::memory_order_release); - timer_.reset(); - if (nav_cmd_publisher_ != nullptr && rclcpp::ok()) { - publish_cycle(true); - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - publish_cycle(true); - } - if (executor_ != nullptr) { - executor_->cancel(); - } - if (spin_thread_.joinable()) { - spin_thread_.join(); - } - nav_cmd_publisher_.reset(); - motion_state_publisher_.reset(); - gait_publisher_.reset(); - hes_subscription_.reset(); - motion_info_subscription_.reset(); - if (executor_ != nullptr && node_ != nullptr) { - executor_->remove_node(node_); - } - node_.reset(); - executor_.reset(); - if (rclcpp::ok()) { - rclcpp::shutdown(); - } - } - -private: - void on_command(const geometry_msgs::Twist& source) { - geometry_msgs::Twist bounded = zero_twist(); - if (std::isfinite(source.linear.x) && std::isfinite(source.linear.y) && - std::isfinite(source.angular.z)) { - bounded.linear.x = clamp(source.linear.x, cfg_.max_linear_x); - bounded.linear.y = clamp(source.linear.y, cfg_.max_linear_y); - bounded.angular.z = clamp(source.angular.z, cfg_.max_angular_z); - } - std::lock_guard lock(state_mutex_); - latest_command_ = bounded; - command_received_at_ = Clock::now(); - have_command_ = true; - } - - void on_motion_state_command(const std_msgs::Int32& source) { - if (motion_state_publisher_ == nullptr || !rclcpp::ok()) return; - drdds::msg::MotionState output; - set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); - output.data.state = source.data; - motion_state_publisher_->publish(output); - logging::warn("published M20 motion-state command", - {logging::Field("state", static_cast(source.data))}); - } - - void on_gait_command(const std_msgs::UInt32& source) { - if (gait_publisher_ == nullptr || !rclcpp::ok()) return; - drdds::msg::Gait output; - set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); - output.data.gait = source.data; - gait_publisher_->publish(output); - logging::info("published M20 gait command", - {logging::Field("gait", static_cast(source.data))}); - } - - void on_hes_status(const drdds::msg::StdMsgInt32& source) { - std::lock_guard lock(state_mutex_); - hes_status_ = vendor_int32_value(source); - have_hes_ = true; - } - - void on_motion_info(const drdds::msg::MotionInfo& source) { - const int motion_state = source.data.motion_state.state; - { - std::lock_guard lock(state_mutex_); - motion_state_value_ = motion_state; - motion_info_received_at_ = Clock::now(); - have_motion_info_ = true; - } - std_msgs::Int32 motion; - motion.data = motion_state; - motion_state_.publish(motion); - std_msgs::UInt32 gait; - gait.data = source.data.gait_state.gait; - gait_state_.publish(gait); - } - - bool safety_ready(Clock::time_point now) const { - std::lock_guard lock(state_mutex_); - if (!have_motion_info_) { - return false; - } - const auto timeout = std::chrono::duration(cfg_.safety_timeout_s); - return now - motion_info_received_at_ <= timeout && - motion_state_value_ == kMotionRlControl && - (!have_hes_ || hes_status_ == 0); - } - - geometry_msgs::Twist fresh_command_or_zero(Clock::time_point now) const { - std::lock_guard lock(state_mutex_); - const auto timeout = std::chrono::duration(cfg_.command_timeout_s); - if (!have_command_ || now - command_received_at_ > timeout) { - return zero_twist(); - } - return latest_command_; - } - - void publish_cycle(bool force_zero) { - const auto now = Clock::now(); - const bool ready = !force_zero && !stopping_.load(std::memory_order_acquire) && - nav_cmd_publisher_ != nullptr && - nav_cmd_publisher_->get_subscription_count() > 0 && - safety_ready(now); - std_msgs::Bool ready_message; - ready_message.data = static_cast(ready); - command_ready_.publish(ready_message); - - if (nav_cmd_publisher_ == nullptr) { - return; - } - const geometry_msgs::Twist command = ready ? fresh_command_or_zero(now) : zero_twist(); - drdds::msg::NavCmd output; - set_vendor_header(output.header, command_sequence_.fetch_add(1), node_->now()); - output.data.x_vel = static_cast(command.linear.x); - output.data.y_vel = static_cast(command.linear.y); - output.data.yaw_vel = static_cast(command.angular.z); - nav_cmd_publisher_->publish(output); - } - - M20ROSBridgeConfig cfg_; - Output command_ready_; - Output motion_state_; - Output gait_state_; - - std::shared_ptr node_; - std::shared_ptr executor_; - rclcpp::Subscription::SharedPtr hes_subscription_; - rclcpp::Subscription::SharedPtr motion_info_subscription_; - rclcpp::Publisher::SharedPtr nav_cmd_publisher_; - rclcpp::Publisher::SharedPtr motion_state_publisher_; - rclcpp::Publisher::SharedPtr gait_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - std::thread spin_thread_; - - mutable std::mutex state_mutex_; - geometry_msgs::Twist latest_command_ = zero_twist(); - Clock::time_point command_received_at_{}; - Clock::time_point motion_info_received_at_{}; - bool have_command_ = false; - bool have_hes_ = false; - bool have_motion_info_ = false; - int motion_state_value_ = 0; - int hes_status_ = 1; - std::atomic stopping_{false}; - std::atomic command_sequence_{0}; -}; - -int main() { - dimos::native::run_with_transport(); - return 0; -} diff --git a/dimos/robot/deeprobotics/m20/bridge/module.py b/dimos/robot/deeprobotics/m20/bridge/module.py deleted file mode 100644 index 51e09ee1f1..0000000000 --- a/dimos/robot/deeprobotics/m20/bridge/module.py +++ /dev/null @@ -1,88 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""NativeModule declaration for the M20 command/state ROS 2 adapter.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING - -from pydantic import Field - -from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.core.stream import In, Out -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.std_msgs.Bool import Bool -from dimos.msgs.std_msgs.Int32 import Int32 -from dimos.msgs.std_msgs.UInt32 import UInt32 -from dimos.robot.deeprobotics.m20.constants import ( - MAX_ANGULAR_Z_RAD_S, - MAX_LINEAR_X_M_S, - MAX_LINEAR_Y_M_S, -) - - -class M20ROSBridgeConfig(NativeModuleConfig): - """Robot-local command/state topics and watchdog settings.""" - - cwd: str | None = "cpp" - executable: str = "build/m20_ros_bridge" - build_command: str | None = "./build.sh" - stdin_config: bool = True - # GOS installs the vendor ROS 2/Foxy and drdds libraries here. NativeModule - # workers do not inherit an interactive shell's ROS setup, so make the - # runtime dependency explicit and reproducible. - extra_env: dict[str, str] = Field( - default_factory=lambda: { - "FASTRTPS_DEFAULT_PROFILES_FILE": "/opt/robot/fastdds.xml", - "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", - "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", - } - ) - - nav_cmd_topic: str = "/NAV_CMD" - motion_state_topic: str = "/MOTION_STATE" - motion_info_topic: str = "/MOTION_INFO" - gait_topic: str = "/GAIT" - hes_status_topic: str = "/HES_STATUS" - node_name: str = "dimos_m20_bridge" - - command_rate_hz: float = Field(default=10.0, gt=0.0) - command_timeout_s: float = Field(default=0.4, gt=0.0) - safety_timeout_s: float = Field(default=2.5, gt=0.0) - max_linear_x: float = Field(default=MAX_LINEAR_X_M_S, gt=0.0) - max_linear_y: float = Field(default=MAX_LINEAR_Y_M_S, gt=0.0) - max_angular_z: float = Field(default=MAX_ANGULAR_Z_RAD_S, gt=0.0) - - -class M20ROSBridge(NativeModule): - """Bridge M20 command and state topics to typed DimOS streams. - - This process runs on GOS and links against the robot's installed Foxy and - ``drdds`` packages. M20PointLio subscribes to lidar and IMU directly; this - bridge carries only low-bandwidth command and robot-state traffic. - """ - - config: M20ROSBridgeConfig - - safe_cmd_vel: In[Twist] - motion_state_cmd: In[Int32] - gait_cmd: In[UInt32] - command_ready: Out[Bool] - motion_state: Out[Int32] - gait_state: Out[UInt32] - - -if TYPE_CHECKING: - M20ROSBridge() diff --git a/dimos/robot/deeprobotics/m20/camera.py b/dimos/robot/deeprobotics/m20/camera.py new file mode 100644 index 0000000000..4e0b18957f --- /dev/null +++ b/dimos/robot/deeprobotics/m20/camera.py @@ -0,0 +1,191 @@ +# 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. + +"""Compressed RTSP camera relay for the Deep Robotics M20.""" + +import threading +import time +from typing import Any + +import av + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.module import Module +from dimos.core.stream import Out +from dimos.msgs.foxglove_msgs.CompressedVideo import CompressedVideo +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.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.tf2_msgs.TFMessage import TFMessage +from dimos.robot.deeprobotics.m20.constants import ( + FRONT_CAMERA_RTSP_URL, + REAR_CAMERA_RTSP_URL, +) +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +# Deep Robotics documents the camera centers relative to the body frame. The +# intrinsics scale the provisional M20/Whale calibration to the 800x600 images +# actually emitted by this robot; the vendor documentation does not publish a +# factory calibration. +_FRONT_CAMERA_XYZ = (0.37646, 0.0, 0.03738) +_REAR_CAMERA_XYZ = (-0.37646, 0.0, 0.03738) +_OPTICAL_ROT = Quaternion(-0.5, 0.5, -0.5, 0.5) +_CAMERA_WIDTH = 800 +_CAMERA_HEIGHT = 600 +_CAMERA_FOCAL_LENGTH = 607.0 * _CAMERA_WIDTH / 1280.0 +_FRONT_CAMERA_INFO = CameraInfo.from_intrinsics( + fx=_CAMERA_FOCAL_LENGTH, + fy=_CAMERA_FOCAL_LENGTH, + cx=_CAMERA_WIDTH * 0.5, + cy=_CAMERA_HEIGHT * 0.5, + width=_CAMERA_WIDTH, + height=_CAMERA_HEIGHT, + frame_id="front_camera_optical", +) +_REAR_CAMERA_INFO = CameraInfo.from_intrinsics( + fx=_CAMERA_FOCAL_LENGTH, + fy=_CAMERA_FOCAL_LENGTH, + cx=_CAMERA_WIDTH * 0.5, + cy=_CAMERA_HEIGHT * 0.5, + width=_CAMERA_WIDTH, + height=_CAMERA_HEIGHT, + frame_id="rear_camera_optical", +) + + +class M20CameraRelay(Module): + """Relay both vendor RTSP cameras as compressed H.265 streams.""" + + front_camera: Out[CompressedVideo] + rear_camera: Out[CompressedVideo] + front_camera_info: Out[CameraInfo] + rear_camera_info: Out[CameraInfo] + tf: Out[TFMessage] + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._stop_event = threading.Event() + self._threads: list[threading.Thread] = [] + + @rpc + def start(self) -> None: + super().start() + self._stop_event.clear() + streams = ( + (FRONT_CAMERA_RTSP_URL, self.front_camera, "m20_front_camera"), + (REAR_CAMERA_RTSP_URL, self.rear_camera, "m20_rear_camera"), + ) + relay_threads = [ + threading.Thread( + target=self._relay, + args=stream, + name=f"{stream[2]}-rtsp", + daemon=True, + ) + for stream in streams + ] + self._threads = [ + threading.Thread( + target=self._publish_camera_metadata, + name="m20-camera-metadata", + daemon=True, + ), + *relay_threads, + ] + for thread in self._threads: + thread.start() + + def _publish_camera_metadata(self) -> None: + while not self._stop_event.is_set(): + now = time.time() + self.tf.publish( + TFMessage( + Transform( + translation=Vector3(*_FRONT_CAMERA_XYZ), + frame_id="base_link", + child_frame_id="front_camera_link", + ts=now, + ), + Transform( + rotation=_OPTICAL_ROT, + frame_id="front_camera_link", + child_frame_id="front_camera_optical", + ts=now, + ), + Transform( + translation=Vector3(*_REAR_CAMERA_XYZ), + rotation=Quaternion(0.0, 0.0, 1.0, 0.0), + frame_id="base_link", + child_frame_id="rear_camera_link", + ts=now, + ), + Transform( + rotation=_OPTICAL_ROT, + frame_id="rear_camera_link", + child_frame_id="rear_camera_optical", + ts=now, + ), + ) + ) + self.front_camera_info.publish(_FRONT_CAMERA_INFO.with_ts(now)) + self.rear_camera_info.publish(_REAR_CAMERA_INFO.with_ts(now)) + self._stop_event.wait(1.0) + + def _relay(self, url: str, output: Out[CompressedVideo], camera_name: str) -> None: + while not self._stop_event.is_set(): + try: + with av.open( + url, + options={"rtsp_transport": "tcp", "fflags": "nobuffer"}, + timeout=(1.0, 1.0), + ) as container: + video = container.streams.video[0] + if video.codec_context.name != "hevc": + raise ValueError(f"expected H.265, got {video.codec_context.name}") + logger.info("M20 camera stream connected", camera=camera_name) + for packet in container.demux(video): + if self._stop_event.is_set(): + return + # The vendor RTSP stream already contains Annex-B access + # units and emits an IDR about every other frame. Publishing + # only IDRs makes every Zenoh sample independently decodable. + if not packet.size or packet.is_corrupt or not packet.is_keyframe: + continue + output.publish( + CompressedVideo( + bytes(packet), + format="h265", + frame_id="", + ts=time.time(), + ) + ) + except (av.FFmpegError, IndexError, ValueError) as exc: + logger.warning( + "M20 camera stream unavailable", + camera=camera_name, + error=str(exc), + ) + self._stop_event.wait(2.0) + + @rpc + def stop(self) -> None: + self._stop_event.set() + for thread in self._threads: + thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + self._threads.clear() + super().stop() diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index bc7ad15135..ad826b84d5 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -29,7 +29,9 @@ from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.nav_msgs.Odometry import Odometry from dimos.msgs.std_msgs.Bool import Bool from dimos.msgs.std_msgs.Int32 import Int32 from dimos.msgs.std_msgs.UInt32 import UInt32 @@ -94,9 +96,11 @@ class M20Connection(Module): command_ready: In[Bool] motion_state: In[Int32] gait_state: In[UInt32] + odometry: In[Odometry] safe_cmd_vel: Out[Twist] motion_state_cmd: Out[Int32] gait_cmd: Out[UInt32] + odom: Out[PoseStamped] def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) @@ -116,6 +120,7 @@ def start(self) -> None: self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) self.register_disposable(Disposable(self.motion_state.subscribe(self._on_motion_state))) self.register_disposable(Disposable(self.gait_state.subscribe(self._on_gait_state))) + self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) self.safe_cmd_vel.publish(Twist.zero()) @rpc @@ -229,6 +234,9 @@ def _on_gait_state(self, msg: UInt32) -> None: self._gait_state = int(msg.data) self._state_condition.notify_all() + def _on_odometry(self, odometry: Odometry) -> None: + self.odom.publish(odometry.to_pose_stamped()) + def _ensure_rl_control(self) -> bool: with self._lock: motion_state = self._motion_state diff --git a/dimos/robot/deeprobotics/m20/constants.py b/dimos/robot/deeprobotics/m20/constants.py index 85062463b0..363e503344 100644 --- a/dimos/robot/deeprobotics/m20/constants.py +++ b/dimos/robot/deeprobotics/m20/constants.py @@ -30,13 +30,6 @@ ROTATION_DIAMETER_M = math.hypot(BODY_LENGTH_M, BODY_WIDTH_M) -# Documented upper command bounds across the supported agile navigation gaits. -# Navigation cruise speed remains controller-owned; these are only the final -# robot-facing envelope for /NAV_CMD. -MAX_LINEAR_X_M_S = 2.0 -MAX_LINEAR_Y_M_S = 1.0 -MAX_ANGULAR_Z_RAD_S = 2.0 - # Vendor AOS H.265 camera streams documented for the M20 internal network. FRONT_CAMERA_RTSP_URL = "rtsp://10.21.31.103:8554/video1" REAR_CAMERA_RTSP_URL = "rtsp://10.21.31.103:8554/video2" diff --git a/dimos/robot/deeprobotics/m20/deploy/README.md b/dimos/robot/deeprobotics/m20/deploy/README.md index a2353abf27..bd7285b6c6 100644 --- a/dimos/robot/deeprobotics/m20/deploy/README.md +++ b/dimos/robot/deeprobotics/m20/deploy/README.md @@ -1,31 +1,35 @@ # M20 deployment -Run `deeprobotics-m20-kronknav-control` on the M20 development computer -(`10.21.31.104`). +Run setup and the complete DimOS stack, including the Rerun bridge, on GOS +(`10.21.31.104`). Setup builds and installs the NOS bridge, runs the RoboSense +driver beside vendor LIO on NOS, and disables the competing lidar driver and +planner by masking their systemd units. These service choices persist across +reboots even when the vendor boot controller explicitly tries to start them. -## One-time robot setup - -From the repository checkout on `10.21.31.104`, run: +## One-time setup ```bash +cd /var/opt/robot/data/dimos-m20-kronknav ./dimos/robot/deeprobotics/m20/deploy/setup.sh ``` -The script configures the other onboard computer to forward lidar data and stop -its competing vendor planner, then configures this computer to expose the lidar -data to DimOS. It is safe to run again and persists across reboots. +The script prompts for the normal GOS `sudo` password and the NOS SSH/`sudo` +password. It is safe to run again. -## Run +## Start DimOS ```bash cd /var/opt/robot/data/dimos-m20-kronknav uv sync --extra all source .venv/bin/activate -dimos --build-native --transport lcm --rerun-host 0.0.0.0 \ +dimos --build-native --robot-ip 10.21.31.106 --rerun-host 0.0.0.0 \ run deeprobotics-m20-kronknav-control --daemon ``` -Connect the viewer: +`--robot-ip` names the NOS Zenoh bridge. `M20Connection` and the camera relay +still use the documented AOS endpoints at `10.21.31.103`. + +Connect the operator viewer directly to the Rerun bridge on GOS: ```bash dimos-viewer \ @@ -33,15 +37,18 @@ dimos-viewer \ --ws-url ws://10.21.31.104:3030/ws ``` -Attach the RPC shell: +The Rerun bridge, camera relay, mapping, planning, and control modules all run +inside the GOS control blueprint. The desktop viewer is only a client. + +Attach the control shell: ```bash -dimos --transport lcm shell +source .venv/bin/activate +dimos shell ``` -Then stand the robot up or lie it down: - ```python app.M20Connection.standup() +app.M20Connection.set_navigation_terrain("stairs") app.M20Connection.liedown() ``` diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor deleted file mode 100644 index 5d9d9facbf..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-multicast-relay-supervisor +++ /dev/null @@ -1,95 +0,0 @@ -#!/usr/bin/env bash -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -set -uo pipefail - -readonly RECEIVE_INTERFACE="eth0" -readonly RECEIVE_ADDRESS="10.21.33.106" -readonly SEND_INTERFACE="eth1" -readonly SEND_ADDRESS="10.21.31.106" -readonly FRONT_LIDAR_ADDRESS="10.21.33.201" -readonly FRONT_MULTICAST_GROUP="224.10.10.201" -readonly MINIMUM_THREAD_COUNT=5 - -relay_pid="" - -stop_relay() { - if [[ -n "$relay_pid" ]] && kill -0 "$relay_pid" 2>/dev/null; then - kill -TERM "$relay_pid" 2>/dev/null || true - wait "$relay_pid" 2>/dev/null || true - fi -} - -handle_stop() { - stop_relay - exit 0 -} - -network_is_ready() { - [[ "$(cat "/sys/class/net/$RECEIVE_INTERFACE/carrier" 2>/dev/null)" == "1" ]] \ - && [[ "$(cat "/sys/class/net/$SEND_INTERFACE/carrier" 2>/dev/null)" == "1" ]] \ - && ip -4 -o address show dev "$RECEIVE_INTERFACE" \ - | grep -Fq "$RECEIVE_ADDRESS/24" \ - && ip -4 -o address show dev "$SEND_INTERFACE" \ - | grep -Fq "$SEND_ADDRESS/24" \ - && ip -4 route get "$FRONT_LIDAR_ADDRESS" \ - from "$RECEIVE_ADDRESS" oif "$RECEIVE_INTERFACE" >/dev/null 2>&1 \ - && ip -4 route get "$FRONT_MULTICAST_GROUP" \ - from "$SEND_ADDRESS" oif "$SEND_INTERFACE" >/dev/null 2>&1 -} - -thread_count() { - find "/proc/$relay_pid/task" -mindepth 1 -maxdepth 1 -type d \ - 2>/dev/null | wc -l -} - -trap handle_stop TERM INT - -for _attempt in $(seq 1 180); do - if network_is_ready; then - break - fi - sleep 0.5 -done - -if ! network_is_ready; then - echo "M20 multicast relay network did not become ready within 90 seconds" >&2 - exit 1 -fi - -/usr/bin/python3 /usr/bin/multicast.py & -relay_pid=$! - -for _attempt in $(seq 1 50); do - if ! kill -0 "$relay_pid" 2>/dev/null; then - wait "$relay_pid" 2>/dev/null - exit $? - fi - if [[ "$(thread_count)" -ge "$MINIMUM_THREAD_COUNT" ]]; then - break - fi - sleep 0.1 -done - -if [[ "$(thread_count)" -lt "$MINIMUM_THREAD_COUNT" ]]; then - echo "M20 multicast relay did not start all four forwarding workers" >&2 - stop_relay - exit 1 -fi - -while kill -0 "$relay_pid" 2>/dev/null; do - if ! network_is_ready; then - echo "M20 multicast relay lost its required network path; restarting" >&2 - stop_relay - exit 1 - fi - if [[ "$(thread_count)" -lt "$MINIMUM_THREAD_COUNT" ]]; then - echo "M20 multicast relay lost a forwarding worker; restarting" >&2 - stop_relay - exit 1 - fi - sleep 1 -done - -wait "$relay_pid" diff --git a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions b/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions deleted file mode 100755 index 00a163f3a9..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/dimos-m20-rsdriver-shm-permissions +++ /dev/null @@ -1,36 +0,0 @@ -#!/usr/bin/env bash -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -set -euo pipefail - -service_pid="${1:-}" -if [[ ! "$service_pid" =~ ^[0-9]+$ ]] || [[ ! -d "/proc/$service_pid" ]]; then - echo "expected the live rsdriver.service MainPID, got: $service_pid" >&2 - exit 1 -fi - -for _attempt in $(seq 1 50); do - driver_pid="$(pgrep --parent "$service_pid" --exact rslidar | head -n 1 || true)" - if [[ -n "$driver_pid" ]]; then - mapfile -t shm_files < <( - { - awk '$NF ~ /^\/dev\/shm\/fastrtps_/ {print $NF}' "/proc/$driver_pid/maps" - for fd in "/proc/$driver_pid"/fd/*; do - readlink -f "$fd" || true - done - } \ - | grep -E '^/dev/shm/fastrtps_([0-9a-f]+|port[0-9]+)(_el)?$' \ - | sort -u - ) - if (( ${#shm_files[@]} >= 4 )); then - chgrp user -- "${shm_files[@]}" - chmod g+rw -- "${shm_files[@]}" - exit 0 - fi - fi - sleep 0.1 -done - -echo "rslidar did not open its Fast DDS shared-memory files within 5 seconds" >&2 -exit 1 diff --git a/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service b/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service new file mode 100644 index 0000000000..cc04ce8d45 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service @@ -0,0 +1,19 @@ +[Unit] +Description=DimOS M20 DrDDS/Zenoh bridge +After=network-online.target localization.service +BindsTo=localization.service +PartOf=localization.service +Wants=network-online.target +StartLimitIntervalSec=0 + +[Service] +Type=simple +Environment="LD_LIBRARY_PATH=/usr/local/lib:/usr/local/lib/aarch64-linux-gnu" +ExecStartPre=/usr/bin/test -e /run/dimos-m20-lio-ready +ExecStart=/usr/local/libexec/m20_drdds_zenoh_bridge +Restart=on-failure +RestartSec=3 +TimeoutStopSec=5 + +[Install] +WantedBy=multi-user.target diff --git a/dimos/robot/deeprobotics/m20/deploy/localization.service.d/10-dimos-lio.conf b/dimos/robot/deeprobotics/m20/deploy/localization.service.d/10-dimos-lio.conf new file mode 100644 index 0000000000..9e3d9feb63 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/deploy/localization.service.d/10-dimos-lio.conf @@ -0,0 +1,17 @@ +[Unit] +After=network-online.target rsdriver.service +Conflicts=hsLidar.service +PartOf=rsdriver.service +Requires=rsdriver.service +Wants=network-online.target + +[Service] +ExecStart= +ExecStart=/opt/robot/share/slam/scripts/boot_lio.sh +ExecStartPre=/usr/bin/rm -f /run/dimos-m20-lio-ready +ExecStartPre=/usr/bin/touch /run/dimos-m20-lio-starting +ExecStartPost=/bin/bash -c 'for _attempt in {1..300}; do if [ /var/opt/robot/data/maps/_boot_lio/.sessions/session_0/lio_odom.pose -nt /run/dimos-m20-lio-starting ]; then /usr/bin/touch /run/dimos-m20-lio-ready; exit 0; fi; sleep 0.1; done; echo "vendor LIO produced no odometry within 30 seconds" >&2; exit 1' +ExecStopPost=/usr/bin/rm -f /run/dimos-m20-lio-ready /run/dimos-m20-lio-starting +Restart=on-failure +RestartSec=3 +TimeoutStartSec=45 diff --git a/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf b/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf deleted file mode 100644 index 6fc1e61abf..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/multicast-relay.service.d/10-dimos-network-readiness.conf +++ /dev/null @@ -1,11 +0,0 @@ -[Unit] -Wants=network-online.target -After=network-online.target -StartLimitIntervalSec=0 - -[Service] -ExecStart= -ExecStart=/usr/local/libexec/dimos-m20-multicast-relay-supervisor -Restart=always -RestartSec=2 -TimeoutStopSec=5 diff --git a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf b/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf deleted file mode 100644 index dff520a1d7..0000000000 --- a/dimos/robot/deeprobotics/m20/deploy/rsdriver.service.d/10-dimos-shm-permissions.conf +++ /dev/null @@ -1,7 +0,0 @@ -# GOS runs rslidar as root so it can request real-time scheduling and publish a -# usable Fast DDS writer on this vendor image. Keep root execution, but expose -# only the driver's shared-memory objects to the normal onboard `user` account. -[Service] -Group=user -UMask=0002 -ExecStartPost=/usr/local/libexec/dimos-m20-rsdriver-shm-permissions $MAINPID diff --git a/dimos/robot/deeprobotics/m20/deploy/setup.sh b/dimos/robot/deeprobotics/m20/deploy/setup.sh index b81937c4c0..63d1abe37d 100755 --- a/dimos/robot/deeprobotics/m20/deploy/setup.sh +++ b/dimos/robot/deeprobotics/m20/deploy/setup.sh @@ -5,50 +5,94 @@ set -euo pipefail deploy_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +bridge_dir="$deploy_dir/../onboard/drdds-zenoh-bridge/cpp" navigation_host="${M20_NAVIGATION_HOST:-user@10.21.31.106}" if ! systemctl cat rsdriver.service >/dev/null 2>&1; then - echo "Run this script on the M20 computer at 10.21.31.104." >&2 + echo "Run this from the DimOS checkout on GOS (10.21.31.104)." >&2 exit 1 fi -echo "Configuring lidar forwarding on ${navigation_host}..." -tar -C "$deploy_dir" -cf - \ - dimos-m20-multicast-relay-supervisor \ - multicast-relay.service.d/10-dimos-network-readiness.conf | \ - ssh -o StrictHostKeyChecking=accept-new -o ConnectTimeout=10 "$navigation_host" ' - set -eu - setup_dir=$(mktemp -d /tmp/dimos-m20-setup.XXXXXX) - trap '\''rm -f "$setup_dir/dimos-m20-multicast-relay-supervisor" "$setup_dir/multicast-relay.service.d/10-dimos-network-readiness.conf"; rmdir "$setup_dir/multicast-relay.service.d" "$setup_dir" 2>/dev/null || true'\'' EXIT - tar -xf - -C "$setup_dir" - sudo install -Dm755 "$setup_dir/dimos-m20-multicast-relay-supervisor" /usr/local/libexec/dimos-m20-multicast-relay-supervisor - sudo install -Dm644 "$setup_dir/multicast-relay.service.d/10-dimos-network-readiness.conf" /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf - sudo rm -f /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf - sudo systemctl daemon-reload - sudo systemctl enable multicast-relay.service - sudo systemctl restart multicast-relay.service - sudo systemctl disable --now planner.service - systemctl is-active --quiet multicast-relay.service - ! systemctl is-active --quiet planner.service - ' +if [[ -z "${DIMOS_LCM_DIR:-}" && -d /var/opt/robot/data/m20-deps/dimos-lcm ]]; then + export DIMOS_LCM_DIR=/var/opt/robot/data/m20-deps/dimos-lcm +fi -echo "Configuring lidar access on 10.21.31.104..." -sudo install -Dm755 \ - "$deploy_dir/dimos-m20-rsdriver-shm-permissions" \ - /usr/local/libexec/dimos-m20-rsdriver-shm-permissions -sudo install -Dm644 \ - "$deploy_dir/rsdriver.service.d/10-dimos-shm-permissions.conf" \ - /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf +echo "Building the M20 DrDDS/Zenoh bridge on GOS..." +"$bridge_dir/build.sh" -if sudo test -e /etc/systemd/system/dimos-m20-fastdds-permissions.path; then - sudo systemctl disable --now dimos-m20-fastdds-permissions.path -fi +stage_dir="$(mktemp -d /tmp/dimos-m20-deploy.XXXXXX)" +trap 'rm -r "$stage_dir"' EXIT +install -Dm755 "$bridge_dir/build/m20_drdds_zenoh_bridge" \ + "$stage_dir/m20_drdds_zenoh_bridge" +install -Dm644 "$deploy_dir/drdds-zenoh-bridge.service" \ + "$stage_dir/drdds-zenoh-bridge.service" +install -Dm644 "$deploy_dir/localization.service.d/10-dimos-lio.conf" \ + "$stage_dir/10-dimos-lio.conf" + +echo "Masking the obsolete GOS lidar driver..." +sudo systemctl mask --now rsdriver.service +sudo systemctl disable --now dimos-m20-fastdds-permissions.path 2>/dev/null || true +sudo systemctl disable --now dimos-m20-fastdds-permissions.service 2>/dev/null || true sudo rm -f \ + /etc/systemd/system/rsdriver.service.d/10-dimos-shm-permissions.conf \ + /usr/local/libexec/dimos-m20-rsdriver-shm-permissions \ /etc/systemd/system/dimos-m20-fastdds-permissions.path \ /etc/systemd/system/dimos-m20-fastdds-permissions.service sudo systemctl daemon-reload -sudo systemctl enable rsdriver.service -sudo systemctl restart rsdriver.service -systemctl is-active --quiet rsdriver.service +! systemctl is-active --quiet rsdriver.service +! systemctl is-enabled --quiet rsdriver.service + +echo "Installing vendor LIO and the bridge on NOS (${navigation_host})..." +tar -C "$stage_dir" -cf - . | ssh \ + -o StrictHostKeyChecking=accept-new \ + -o ConnectTimeout=10 \ + "$navigation_host" ' + set -eu + setup_dir=$(mktemp -d /tmp/dimos-m20-setup.XXXXXX) + trap '\''rm -r "$setup_dir"'\'' EXIT + tar -xf - -C "$setup_dir" + + sudo install -Dm755 "$setup_dir/m20_drdds_zenoh_bridge" \ + /usr/local/libexec/m20_drdds_zenoh_bridge + sudo install -Dm644 "$setup_dir/drdds-zenoh-bridge.service" \ + /etc/systemd/system/drdds-zenoh-bridge.service + sudo install -Dm644 "$setup_dir/10-dimos-lio.conf" \ + /etc/systemd/system/localization.service.d/10-dimos-lio.conf + + sudo rm -f \ + /etc/systemd/system/drdds-zenoh-bridge.service.d/connect.conf \ + /etc/systemd/system/localization.service.d/lio.conf \ + /etc/systemd/system/planner.service.d/10-dimos-command-ownership.conf \ + /etc/systemd/system/multicast-relay.service.d/10-dimos-network-readiness.conf \ + /usr/local/libexec/dimos-m20-multicast-relay-supervisor + sudo systemctl daemon-reload + + sudo systemctl stop drdds-zenoh-bridge.service localization.service + sudo systemctl mask --now multicast-relay.service + sudo systemctl mask --now hsLidar.service + sudo systemctl mask --now planner.service + + sudo systemctl unmask rsdriver.service localization.service \ + drdds-zenoh-bridge.service + sudo systemctl enable rsdriver.service localization.service \ + drdds-zenoh-bridge.service + sudo systemctl restart rsdriver.service + sudo systemctl restart localization.service + sudo systemctl restart drdds-zenoh-bridge.service + + systemctl is-active --quiet rsdriver.service + systemctl is-active --quiet localization.service + systemctl is-active --quiet drdds-zenoh-bridge.service + systemctl is-enabled --quiet rsdriver.service + systemctl is-enabled --quiet localization.service + systemctl is-enabled --quiet drdds-zenoh-bridge.service + test -e /run/dimos-m20-lio-ready + ! systemctl is-active --quiet hsLidar.service + ! systemctl is-active --quiet multicast-relay.service + ! systemctl is-active --quiet planner.service + test "$(systemctl is-enabled hsLidar.service)" = masked + test "$(systemctl is-enabled multicast-relay.service)" = masked + test "$(systemctl is-enabled planner.service)" = masked + ' echo "M20 setup complete." diff --git a/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/CMakeLists.txt new file mode 100644 index 0000000000..abe400b0cb --- /dev/null +++ b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.14) +project(m20_drdds_zenoh_bridge CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# dimos_lcm: generated C++ LCM message headers (sensor_msgs/, nav_msgs/, ...). +# These are header-only (inline encode()/getHash()); we do NOT link liblcm — the +# LCM library is only the udpm transport, and the carrier here is Zenoh instead. +# Offline builds (the M20 boxes have no clean internet): pass a local checkout via +# -DFETCHCONTENT_SOURCE_DIR_DIMOS_LCM=/path/to/dimos-lcm +if(DEFINED DIMOS_LCM_DIR) + set(dimos_lcm_SOURCE_DIR ${DIMOS_LCM_DIR}) +else() + include(FetchContent) + FetchContent_Declare(dimos_lcm + GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git + GIT_TAG 0a1c24710ce2f7a569e1673617102cc5254a75e2 + ) + FetchContent_GetProperties(dimos_lcm) + if(NOT dimos_lcm_POPULATED) + FetchContent_Populate(dimos_lcm) + endif() +endif() + +add_executable(m20_drdds_zenoh_bridge main.cpp) + +# drdds/dridl live under /usr/local (DeepRobotics SDK); zenoh-c (libzenohc.so + +# headers) is installed to /usr/local too. +target_include_directories(m20_drdds_zenoh_bridge PRIVATE + /usr/local/include + /usr/local/include/drdds + /usr/local/include/dridl + ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs +) + +target_link_directories(m20_drdds_zenoh_bridge PRIVATE + /usr/local/lib +) + +target_link_libraries(m20_drdds_zenoh_bridge + drdds + fastrtps + fastcdr + zenohc + pthread +) +target_compile_options(m20_drdds_zenoh_bridge PRIVATE -Wall -Wextra -Wpedantic) + +install(TARGETS m20_drdds_zenoh_bridge DESTINATION bin) diff --git a/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/build.sh b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/build.sh new file mode 100755 index 0000000000..b25a5e4d3e --- /dev/null +++ b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/build.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env bash +# Build the bidirectional DrDDS/Zenoh bridge on an M20 AArch64 computer. +set -euo pipefail + +bridge_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +cmake_args=( + -S "$bridge_dir" + -B "$bridge_dir/build" + -DCMAKE_BUILD_TYPE=Release +) +if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then + cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") +elif [[ -d /tmp/dimos-lcm ]]; then + cmake_args+=("-DDIMOS_LCM_DIR=/tmp/dimos-lcm") +fi + +cmake "${cmake_args[@]}" +cmake --build "$bridge_dir/build" --parallel "${M20_BUILD_JOBS:-4}" +echo "built: $bridge_dir/build/m20_drdds_zenoh_bridge" diff --git a/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp new file mode 100644 index 0000000000..89276a8a50 --- /dev/null +++ b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp @@ -0,0 +1,714 @@ +// Copyright 2026 Dimensional Inc. +// SPDX-License-Identifier: Apache-2.0 +// +// DeepRobotics M20 DrDDS <-> DimOS Zenoh bridge. +// +// Runs on NOS. It forwards the vendor LIO body-frame cloud and odometry to +// typed DimOS Zenoh streams, and forwards the small DimOS control streams back +// to the robot's onboard Fast-DDS fork ("drdds"). +// +// It also receives the small DimOS command streams over Zenoh and publishes the +// vendor DrDDS command topics. Dense clouds never cross an LCM boundary. +// +// This executable is deliberately fixed to the M20 deployment. Run it as root +// for access to the robot's root-owned Fast-DDS SHM writers. + +#include "drdds/core/drdds_core.h" + +#include "dridl/sensor_msgs/msg/PointCloud2.h" +#include "dridl/sensor_msgs/msg/PointCloud2PubSubTypes.h" +#include "dridl/nav_msgs/msg/Odometry.h" +#include "dridl/nav_msgs/msg/OdometryPubSubTypes.h" + +#include "dridl/dr_msgs/msg/GaitPubSubTypes.h" +#include "dridl/dr_msgs/msg/MotionInfoPubSubTypes.h" +#include "dridl/dr_msgs/msg/MotionStatePubSubTypes.h" +#include "dridl/dr_msgs/msg/NavCmdPubSubTypes.h" +#include "dridl/dr_msgs/msg/StdMsgInt32PubSubTypes.h" + +#include + +#include "geometry_msgs/Quaternion.hpp" +#include "geometry_msgs/Twist.hpp" +#include "geometry_msgs/Vector3.hpp" +#include "nav_msgs/Odometry.hpp" +#include "sensor_msgs/PointCloud2.hpp" +#include "sensor_msgs/PointField.hpp" +#include "std_msgs/Bool.hpp" +#include "std_msgs/Int32.hpp" +#include "std_msgs/UInt32.hpp" +#include "tf2_msgs/TFMessage.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static std::atomic g_running{true}; +static void on_signal(int) { g_running.store(false); } + +constexpr int kDomain = 0; +constexpr char kDrddsNetwork[] = "10.21.33.106"; +constexpr char kZenohListen[] = "tcp/0.0.0.0:7447"; + +constexpr char kBodyTopic[] = "/SLAM_CLOUD_REGISTERED_BODY"; +constexpr char kOdometryTopic[] = "/SLAM_ODOM"; +constexpr char kNavCmdTopic[] = "/NAV_CMD"; +constexpr char kMotionStateTopic[] = "/MOTION_STATE"; +constexpr char kMotionInfoTopic[] = "/MOTION_INFO"; +constexpr char kGaitTopic[] = "/GAIT"; +constexpr char kHesStatusTopic[] = "/HES_STATUS"; + +constexpr char kBodyKey[] = "dimos/slam_body_points/sensor_msgs.PointCloud2"; +constexpr char kOdometryKey[] = "dimos/slam_odom/nav_msgs.Odometry"; +constexpr char kTfKey[] = "dimos/tf/tf2_msgs.TFMessage"; +constexpr char kSafeCmdVelKey[] = "dimos/safe_cmd_vel/geometry_msgs.Twist"; +constexpr char kMotionStateCmdKey[] = "dimos/motion_state_cmd/std_msgs.Int32"; +constexpr char kGaitCmdKey[] = "dimos/gait_cmd/std_msgs.UInt32"; +constexpr char kCommandReadyKey[] = "dimos/command_ready/std_msgs.Bool"; +constexpr char kMotionStateKey[] = "dimos/motion_state/std_msgs.Int32"; +constexpr char kGaitStateKey[] = "dimos/gait_state/std_msgs.UInt32"; + +constexpr double kCommandRateHz = 10.0; +constexpr double kCommandTimeoutS = 0.4; +constexpr double kStateTimeoutS = 2.5; +constexpr double kMaxLinearX = 2.0; +constexpr double kMaxLinearY = 1.0; +constexpr double kMaxAngularZ = 2.0; + +// One wired output: its fixed Zenoh key plus counters shown in the status line. +struct Port { + Port(std::string key_value, std::string label_value) + : key(std::move(key_value)), label(std::move(label_value)) {} + + std::string key; // zenoh key expr, e.g. "dimos/aligned_points/sensor_msgs.PointCloud2" + std::string label; // short name for logs + std::atomic n{0}; + std::atomic bytes{0}; + std::atomic described{false}; + std::function matched; // GetMatchedCount() of the underlying drdds reader +}; + +struct InputPort { + InputPort(std::string key_value, std::string label_value, + std::function callback) + : key(std::move(key_value)), + label(std::move(label_value)), + handler(std::move(callback)) {} + + std::string key; + std::string label; + std::function handler; + std::atomic n{0}; + std::atomic bytes{0}; + std::atomic decode_errors{0}; +}; + +struct TfGate { + std::mutex mutex; + std::condition_variable ready; + int64_t latest_stamp_ns = std::numeric_limits::min(); +}; + +// ----------------------------------------------------------------- zenoh out -- +static z_owned_session_t g_session; +static std::mutex g_pub_mx; +static std::map g_pubs; // key -> cached publisher +static std::map g_subs; + +// Get or declare the cached publisher for a key, matching DimOS's default +// topic QoS policy in core/transport_factory.py: +// - high-rate clouds/images -> DROP congestion control, so a momentarily-slow +// link (e.g. WiFi) drops stale frames instead of building a reliable +// in-order backlog that makes every subscriber lag behind realtime. +// - everything else (odometry, etc.) -> BLOCK, never drop under congestion. +// (zenoh-c 1.2 reliability is a no-op on the wire, but we set it for parity.) +static const z_loaned_publisher_t* get_pub(const std::string& key) { + std::lock_guard lk(g_pub_mx); + auto it = g_pubs.find(key); + if (it == g_pubs.end()) { + z_view_keyexpr_t ke; + if (z_view_keyexpr_from_str(&ke, key.c_str()) != Z_OK) { + fprintf(stderr, "[bridge] bad key expr '%s'\n", key.c_str()); + return nullptr; + } + const bool is_stream = key.find("sensor_msgs.PointCloud2") != std::string::npos || + key.find("sensor_msgs.Image") != std::string::npos; + z_publisher_options_t opts; + z_publisher_options_default(&opts); + if (is_stream) { + opts.congestion_control = Z_CONGESTION_CONTROL_DROP; + opts.reliability = Z_RELIABILITY_BEST_EFFORT; + } else { + opts.congestion_control = Z_CONGESTION_CONTROL_BLOCK; + opts.reliability = Z_RELIABILITY_RELIABLE; + } + z_owned_publisher_t pub; + if (z_declare_publisher(z_loan(g_session), &pub, z_loan(ke), &opts) != Z_OK) { + fprintf(stderr, "[bridge] declare_publisher failed for '%s'\n", key.c_str()); + return nullptr; + } + it = g_pubs.emplace(key, pub).first; + } + return z_loan(it->second); +} + +// LCM-encode a dimos_lcm message and publish the raw bytes on the port's key. +template +static void publish_zenoh(Port* p, const T& msg) { + const z_loaned_publisher_t* pub = get_pub(p->key); + if (pub == nullptr) { return; } + const int len = msg.getEncodedSize(); + if (len < 0) { return; } + std::vector buf(static_cast(len)); + if (msg.encode(buf.data(), 0, len) != len) { + fprintf(stderr, "[bridge] encode failed for '%s'\n", p->key.c_str()); + return; + } + z_owned_bytes_t payload; + z_bytes_copy_from_buf(&payload, buf.data(), static_cast(len)); + z_publisher_put_options_t po; + z_publisher_put_options_default(&po); + z_publisher_put(pub, z_move(payload), &po); + p->n.fetch_add(1, std::memory_order_relaxed); + p->bytes.fetch_add(len, std::memory_order_relaxed); +} + +template +static bool decode_lcm(const uint8_t* data, size_t len, T* out) { + if (len > static_cast(std::numeric_limits::max())) { return false; } + return out->decode(data, 0, static_cast(len)) >= 0; +} + +static void on_zenoh_sample(z_loaned_sample_t* sample, void* context) { + auto* port = static_cast(context); + if (sample == nullptr || port == nullptr || z_sample_kind(sample) != Z_SAMPLE_KIND_PUT) { + return; + } + const z_loaned_bytes_t* payload = z_sample_payload(sample); + const size_t len = z_bytes_len(payload); + std::vector buffer(len); + z_bytes_reader_t reader = z_bytes_get_reader(payload); + if (z_bytes_reader_read(&reader, buffer.data(), len) != len) { + port->decode_errors.fetch_add(1, std::memory_order_relaxed); + return; + } + if (port->handler(buffer.data(), buffer.size())) { + port->n.fetch_add(1, std::memory_order_relaxed); + port->bytes.fetch_add(static_cast(len), std::memory_order_relaxed); + } else { + port->decode_errors.fetch_add(1, std::memory_order_relaxed); + } +} + +static bool declare_zenoh_subscriber(InputPort* port) { + z_view_keyexpr_t key; + if (z_view_keyexpr_from_str(&key, port->key.c_str()) != Z_OK) { + fprintf(stderr, "[bridge] bad subscriber key expr '%s'\n", port->key.c_str()); + return false; + } + z_owned_closure_sample_t callback; + z_closure(&callback, on_zenoh_sample, nullptr, port); + z_subscriber_options_t options; + z_subscriber_options_default(&options); + z_owned_subscriber_t subscriber; + if (z_declare_subscriber(z_loan(g_session), &subscriber, z_loan(key), + z_move(callback), &options) != Z_OK) { + fprintf(stderr, "[bridge] declare_subscriber failed for '%s'\n", port->key.c_str()); + return false; + } + g_subs.emplace(port->key, subscriber); + fprintf(stderr, "[bridge] %s: %s -> Zenoh input\n", port->label.c_str(), + port->key.c_str()); + return true; +} + +// ----------------------------------------------------- drdds -> dimos_lcm conv -- +// (identical field-for-field copies to ../../dds/cpp/main.cpp; the drdds and +// dimos_lcm ROS-message layouts match, so these are straight member copies.) + +static std_msgs::Header to_lcm_header(const std_msgs::msg::Header& h) { + static std::atomic seq{0}; + std_msgs::Header out; + out.seq = seq.fetch_add(1, std::memory_order_relaxed); + out.stamp.sec = h.stamp().sec(); + out.stamp.nsec = static_cast(h.stamp().nanosec()); + out.frame_id = h.frame_id(); + return out; +} + +static int64_t stamp_ns(const std_msgs::msg::Header& h) { + return static_cast(h.stamp().sec()) * 1'000'000'000LL + + static_cast(h.stamp().nanosec()); +} + +static void on_pointcloud(const sensor_msgs::msg::PointCloud2* m, Port* p, + TfGate* tf_gate) { + if (m == nullptr) { return; } + if (tf_gate != nullptr) { + const int64_t cloud_stamp_ns = stamp_ns(m->header()); + std::unique_lock lock(tf_gate->mutex); + if (!tf_gate->ready.wait_for(lock, std::chrono::milliseconds(200), [&] { + return tf_gate->latest_stamp_ns >= cloud_stamp_ns; + })) { + return; + } + } + if (!p->described.exchange(true)) { + fprintf(stderr, "[bridge] first body cloud: frame=%s points=%ux%u step=%u\n", + m->header().frame_id().c_str(), m->width(), m->height(), m->point_step()); + } + sensor_msgs::PointCloud2 pc; + pc.header = to_lcm_header(m->header()); + pc.height = m->height(); + pc.width = m->width(); + pc.is_bigendian = m->is_bigendian(); + pc.point_step = m->point_step(); + pc.row_step = m->row_step(); + pc.is_dense = m->is_dense(); + + const auto& fields = m->fields(); + pc.fields_length = static_cast(fields.size()); + pc.fields.resize(fields.size()); + for (size_t i = 0; i < fields.size(); ++i) { + pc.fields[i].name = fields[i].name(); + pc.fields[i].offset = fields[i].offset(); + pc.fields[i].datatype = static_cast(fields[i].datatype()); + pc.fields[i].count = fields[i].count(); + } + + const auto& data = m->data(); + pc.data.resize(data.size()); + if (!data.empty()) { + std::memcpy(pc.data.data(), data.data(), data.size()); + } + pc.data_length = static_cast(pc.data.size()); + publish_zenoh(p, pc); +} + +static void on_odometry(const nav_msgs::msg::Odometry* m, Port* odometry_port, + Port* tf_port, TfGate* tf_gate) { + if (m == nullptr) { return; } + if (!odometry_port->described.exchange(true)) { + fprintf(stderr, "[bridge] first odometry: frame=%s child=%s\n", + m->header().frame_id().c_str(), m->child_frame_id().c_str()); + } + nav_msgs::Odometry out; + out.header = to_lcm_header(m->header()); + out.child_frame_id = m->child_frame_id(); + + const auto& pose = m->pose().pose(); + out.pose.pose.position.x = pose.position().x(); + out.pose.pose.position.y = pose.position().y(); + out.pose.pose.position.z = pose.position().z(); + out.pose.pose.orientation.x = pose.orientation().x(); + out.pose.pose.orientation.y = pose.orientation().y(); + out.pose.pose.orientation.z = pose.orientation().z(); + out.pose.pose.orientation.w = pose.orientation().w(); + + const auto& tw = m->twist().twist(); + out.twist.twist.linear.x = tw.linear().x(); + out.twist.twist.linear.y = tw.linear().y(); + out.twist.twist.linear.z = tw.linear().z(); + out.twist.twist.angular.x = tw.angular().x(); + out.twist.twist.angular.y = tw.angular().y(); + out.twist.twist.angular.z = tw.angular().z(); + + const auto& pcov = m->pose().covariance(); + const auto& tcov = m->twist().covariance(); + for (int i = 0; i < 36; ++i) { + out.pose.covariance[i] = pcov[i]; + out.twist.covariance[i] = tcov[i]; + } + // Publish the transform from the same NOS callback, before the matching + // body cloud can reach the mapper. Generating it one process later from + // `slam_odom` lets the cloud handler block the mapper's dispatch loop while + // the required TF is already queued behind it. + tf2_msgs::TFMessage tf; + tf.transforms_length = 1; + tf.transforms.resize(1); + auto& transform = tf.transforms[0]; + transform.header = out.header; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = out.pose.pose.position.x; + transform.transform.translation.y = out.pose.pose.position.y; + transform.transform.translation.z = out.pose.pose.position.z; + transform.transform.rotation = out.pose.pose.orientation; + publish_zenoh(tf_port, tf); + { + std::lock_guard lock(tf_gate->mutex); + tf_gate->latest_stamp_ns = stamp_ns(m->header()); + } + tf_gate->ready.notify_all(); + publish_zenoh(odometry_port, out); +} + +// ---------------------------------------------------------- Zenoh -> DrDDS -- + +namespace { + +using Clock = std::chrono::steady_clock; +constexpr int64_t kNanosecondsPerSecond = 1'000'000'000LL; +constexpr int kMotionRlControl = 17; + +double clamp(double value, double limit) { + return std::max(-limit, std::min(limit, value)); +} + +geometry_msgs::Twist zero_twist() { + geometry_msgs::Twist result; + result.linear.x = 0.0; + result.linear.y = 0.0; + result.linear.z = 0.0; + result.angular.x = 0.0; + result.angular.y = 0.0; + result.angular.z = 0.0; + return result; +} + +void set_vendor_header(drdds::msg::MetaType& header, uint64_t frame_id) { + const auto now = std::chrono::system_clock::now().time_since_epoch(); + const int64_t total_ns = + std::chrono::duration_cast(now).count(); + header.frame_id(frame_id); + header.timestamp().sec(static_cast(total_ns / kNanosecondsPerSecond)); + header.timestamp().nsec(static_cast(total_ns % kNanosecondsPerSecond)); +} + +class M20ControlBridge { +public: + M20ControlBridge(Port* command_ready, Port* motion_state, Port* gait_state) + : command_ready_(command_ready), + motion_state_(motion_state), + gait_state_(gait_state) {} + + ~M20ControlBridge() { stop(); } + + void start() { + // Robot motion runs on AOS, so these topics cross to NOS over the + // vendor's UDP DDS transport. The LIO outputs below remain local SHM. + constexpr bool use_shm = false; + hes_subscription_ = std::make_unique< + DrDDSChannel>( + [this](const drdds::msg::StdMsgInt32* msg) { on_hes_status(msg); }, + kHesStatusTopic, kDomain, use_shm, "rt"); + motion_info_subscription_ = std::make_unique< + DrDDSChannel>( + [this](const drdds::msg::MotionInfo* msg) { on_motion_info(msg); }, + kMotionInfoTopic, kDomain, use_shm, "rt"); + nav_cmd_publisher_ = + std::make_unique>( + kNavCmdTopic, kDomain, use_shm, "rt"); + motion_state_publisher_ = + std::make_unique>( + kMotionStateTopic, kDomain, use_shm, "rt"); + gait_publisher_ = std::make_unique>( + kGaitTopic, kDomain, use_shm, "rt"); + command_ready_->matched = [this] { return nav_cmd_publisher_->GetMatchedCount(); }; + motion_state_->matched = + [this] { return motion_info_subscription_->GetMatchedCount(); }; + gait_state_->matched = + [this] { return motion_info_subscription_->GetMatchedCount(); }; + command_thread_ = std::thread([this]() { command_loop(); }); + fprintf(stderr, "[bridge] bidirectional M20 command/state path started\n"); + } + + void stop() { + if (stopped_.exchange(true)) { return; } + if (command_thread_.joinable()) { command_thread_.join(); } + publish_cycle(true); + nav_cmd_publisher_.reset(); + motion_state_publisher_.reset(); + gait_publisher_.reset(); + hes_subscription_.reset(); + motion_info_subscription_.reset(); + } + + bool on_command(const uint8_t* data, size_t len) { + geometry_msgs::Twist source; + if (!decode_lcm(data, len, &source)) { return false; } + geometry_msgs::Twist bounded = zero_twist(); + if (std::isfinite(source.linear.x) && std::isfinite(source.linear.y) && + std::isfinite(source.angular.z)) { + bounded.linear.x = clamp(source.linear.x, kMaxLinearX); + bounded.linear.y = clamp(source.linear.y, kMaxLinearY); + bounded.angular.z = clamp(source.angular.z, kMaxAngularZ); + } + std::lock_guard lock(state_mutex_); + latest_command_ = bounded; + command_received_at_ = Clock::now(); + command_path_active_ = true; + return true; + } + + bool on_motion_state_command(const uint8_t* data, size_t len) { + std_msgs::Int32 source; + if (!decode_lcm(data, len, &source)) { return false; } + if (motion_state_publisher_ == nullptr) { return true; } + drdds::msg::MotionState output; + set_vendor_header(output.header(), command_sequence_.fetch_add(1)); + output.data().state(source.data); + motion_state_publisher_->Write(&output); + fprintf(stderr, "[bridge] published motion-state command: %d\n", source.data); + return true; + } + + bool on_gait_command(const uint8_t* data, size_t len) { + std_msgs::UInt32 source; + if (!decode_lcm(data, len, &source)) { return false; } + if (gait_publisher_ == nullptr) { return true; } + drdds::msg::Gait output; + set_vendor_header(output.header(), command_sequence_.fetch_add(1)); + output.data().gait(source.data); + gait_publisher_->Write(&output); + fprintf(stderr, "[bridge] published gait command: %u\n", source.data); + return true; + } + +private: + void on_hes_status(const drdds::msg::StdMsgInt32* source) { + if (source == nullptr) { return; } + std::lock_guard lock(state_mutex_); + hes_status_ = source->value(); + have_hes_ = true; + } + + void on_motion_info(const drdds::msg::MotionInfo* source) { + if (source == nullptr) { return; } + const int motion_state = source->data().motion_state().state(); + { + std::lock_guard lock(state_mutex_); + motion_state_value_ = motion_state; + motion_info_received_at_ = Clock::now(); + have_motion_info_ = true; + } + std_msgs::Int32 motion; + motion.data = motion_state; + publish_zenoh(motion_state_, motion); + std_msgs::UInt32 gait; + gait.data = source->data().gait_state().gait(); + publish_zenoh(gait_state_, gait); + } + + bool robot_ready(Clock::time_point now) const { + std::lock_guard lock(state_mutex_); + const auto timeout = std::chrono::duration(kStateTimeoutS); + return have_motion_info_ && now - motion_info_received_at_ <= timeout && + motion_state_value_ == kMotionRlControl && (!have_hes_ || hes_status_ == 0); + } + + void publish_nav(const geometry_msgs::Twist& command) { + if (nav_cmd_publisher_ == nullptr) { return; } + drdds::msg::NavCmd output; + set_vendor_header(output.header(), command_sequence_.fetch_add(1)); + output.data().x_vel(static_cast(command.linear.x)); + output.data().y_vel(static_cast(command.linear.y)); + output.data().yaw_vel(static_cast(command.angular.z)); + nav_cmd_publisher_->Write(&output); + } + + void publish_final_zero() { + const geometry_msgs::Twist zero = zero_twist(); + publish_nav(zero); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + publish_nav(zero); + } + + void command_loop() { + const auto period = std::chrono::duration(1.0 / kCommandRateHz); + while (!stopped_.load()) { + const auto next = Clock::now() + period; + publish_cycle(false); + std::this_thread::sleep_until(next); + } + } + + void publish_cycle(bool stopping) { + const auto now = Clock::now(); + const bool ready = !stopping && nav_cmd_publisher_ != nullptr && + nav_cmd_publisher_->GetMatchedCount() > 0 && + robot_ready(now); + std_msgs::Bool ready_message; + ready_message.data = static_cast(ready); + publish_zenoh(command_ready_, ready_message); + + geometry_msgs::Twist command = zero_twist(); + bool publish = false; + bool release = false; + { + std::lock_guard lock(state_mutex_); + if (command_path_active_) { + const auto timeout = std::chrono::duration(kCommandTimeoutS); + if (stopping || now - command_received_at_ > timeout) { + command_path_active_ = false; + release = true; + } else { + command = ready ? latest_command_ : zero_twist(); + publish = true; + } + } + } + if (release) { + publish_final_zero(); + } else if (publish) { + publish_nav(command); + } + } + + Port* command_ready_; + Port* motion_state_; + Port* gait_state_; + std::unique_ptr> hes_subscription_; + std::unique_ptr> + motion_info_subscription_; + std::unique_ptr> nav_cmd_publisher_; + std::unique_ptr> + motion_state_publisher_; + std::unique_ptr> gait_publisher_; + std::thread command_thread_; + mutable std::mutex state_mutex_; + geometry_msgs::Twist latest_command_ = zero_twist(); + Clock::time_point command_received_at_{}; + Clock::time_point motion_info_received_at_{}; + bool command_path_active_ = false; + bool have_hes_ = false; + bool have_motion_info_ = false; + int motion_state_value_ = 0; + int hes_status_ = 1; + std::atomic stopped_{false}; + std::atomic command_sequence_{0}; +}; + +} // namespace + +int main() { + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + // NOS is the fixed hub between robot-side peers and the operator client. + // Router mode prevents offboard clients from gossiping directly with every + // GOS process (and then trying those processes' loopback-only locators). + z_owned_config_t cfg; + z_config_default(&cfg); + zc_config_insert_json5(z_config_loan_mut(&cfg), "mode", "\"router\""); + zc_config_insert_json5(z_config_loan_mut(&cfg), + "scouting/multicast/enabled", "false"); + zc_config_insert_json5(z_config_loan_mut(&cfg), + "scouting/gossip/enabled", "false"); + const std::string listen = "[\"" + std::string(kZenohListen) + "\"]"; + zc_config_insert_json5(z_config_loan_mut(&cfg), "listen/endpoints", listen.c_str()); + if (z_open(&g_session, z_move(cfg), nullptr) != Z_OK) { + fprintf(stderr, "[bridge] zenoh session open failed\n"); + return 1; + } + fprintf(stderr, "[bridge] listening on %s\n", kZenohListen); + + DrDDSManager::Init(kDomain, kDrddsNetwork); + + TfGate tf_gate; + Port body(kBodyKey, "body"); + Port tf(kTfKey, "tf"); + Port odometry(kOdometryKey, "odometry"); + Port command_ready(kCommandReadyKey, "command_ready"); + Port motion_state(kMotionStateKey, "motion_state"); + Port gait_state(kGaitStateKey, "gait_state"); + std::vector output_ports{ + &body, &tf, &odometry, &command_ready, &motion_state, &gait_state, + }; + + constexpr bool use_shm = true; + auto body_channel = + std::make_unique>( + [&body, &tf_gate](const sensor_msgs::msg::PointCloud2* message) { + on_pointcloud(message, &body, &tf_gate); + }, + kBodyTopic, kDomain, use_shm, "rt"); + auto odometry_channel = + std::make_unique>( + [&odometry, &tf, &tf_gate](const nav_msgs::msg::Odometry* message) { + on_odometry(message, &odometry, &tf, &tf_gate); + }, + kOdometryTopic, kDomain, use_shm, "rt"); + body.matched = [&body_channel] { return body_channel->GetMatchedCount(); }; + odometry.matched = [&odometry_channel] { + return odometry_channel->GetMatchedCount(); + }; + tf.matched = odometry.matched; + + M20ControlBridge control(&command_ready, &motion_state, &gait_state); + control.start(); + + InputPort safe_cmd_vel( + kSafeCmdVelKey, "safe_cmd_vel", + [&control](const uint8_t* data, size_t len) { + return control.on_command(data, len); + }); + InputPort motion_state_cmd( + kMotionStateCmdKey, "motion_state_cmd", + [&control](const uint8_t* data, size_t len) { + return control.on_motion_state_command(data, len); + }); + InputPort gait_cmd( + kGaitCmdKey, "gait_cmd", + [&control](const uint8_t* data, size_t len) { + return control.on_gait_command(data, len); + }); + std::vector input_ports{&safe_cmd_vel, &motion_state_cmd, &gait_cmd}; + for (InputPort* port : input_ports) { + if (!declare_zenoh_subscriber(port)) { g_running.store(false); } + } + + fprintf(stderr, "[bridge] body: rt%s -> %s\n", kBodyTopic, kBodyKey); + fprintf(stderr, "[bridge] odometry: rt%s -> %s + %s\n", kOdometryTopic, + kOdometryKey, kTfKey); + fprintf(stderr, "[bridge] Fast-DDS SHM enabled, domain %d\n", kDomain); + long t = 0; + while (g_running.load()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + ++t; + std::string line = "t=" + std::to_string(t) + "s"; + for (const Port* p : output_ports) { + char b[96]; + const int m = p->matched ? p->matched() : -1; + snprintf(b, sizeof(b), " %s[m=%d n=%ld %.1fMB]", p->label.c_str(), m, p->n.load(), + p->bytes.load() / 1e6); + line += b; + } + for (const InputPort* p : input_ports) { + char b[96]; + snprintf(b, sizeof(b), " %s[in=%ld err=%ld]", p->label.c_str(), p->n.load(), + p->decode_errors.load()); + line += b; + } + fprintf(stderr, "%s\n", line.c_str()); + } + + fprintf(stderr, "[bridge] shutting down\n"); + for (auto& entry : g_subs) { z_drop(z_move(entry.second)); } + g_subs.clear(); + control.stop(); + body_channel.reset(); + odometry_channel.reset(); + DrDDSManager::Delete(); + { + std::lock_guard lock(g_pub_mx); + for (auto& entry : g_pubs) { z_drop(z_move(entry.second)); } + g_pubs.clear(); + } + z_drop(z_move(g_session)); + return 0; +} diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt b/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt deleted file mode 100644 index 65dd18ec97..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/CMakeLists.txt +++ /dev/null @@ -1,112 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -cmake_minimum_required(VERSION 3.14) -project(m20_pointlio CXX) - -set(CMAKE_CXX_STANDARD 20) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - -find_package(ament_cmake REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(OpenMP QUIET) -find_package(PCL 1.8 REQUIRED COMPONENTS common filters) -find_package(PkgConfig REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) -pkg_check_modules(LCM REQUIRED IMPORTED_TARGET lcm) - -include(FetchContent) - -if(DEFINED DIMOS_LCM_DIR) - set(dimos_lcm_SOURCE_DIR ${DIMOS_LCM_DIR}) -else() - FetchContent_Declare(dimos_lcm - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-lcm.git - GIT_TAG 0a1c24710ce2f7a569e1673617102cc5254a75e2 - ) - FetchContent_GetProperties(dimos_lcm) - if(NOT dimos_lcm_POPULATED) - FetchContent_Populate(dimos_lcm) - endif() -endif() - -if(NOT DEFINED POINTLIO_DIR) - FetchContent_Declare(pointlio - GIT_REPOSITORY https://github.com/dimensionalOS/dimos-module-pointlio.git - GIT_TAG 82ef3a327347e2866e981bd95c8bece8b72903cf - ) - FetchContent_GetProperties(pointlio) - if(NOT pointlio_POPULATED) - FetchContent_Populate(pointlio) - endif() - set(POINTLIO_DIR ${pointlio_SOURCE_DIR}) -endif() - -set(POINTLIO_SOURCE_DIR ${POINTLIO_DIR}) -file(READ "${POINTLIO_SOURCE_DIR}/src/laserMapping.hpp" pointlio_mapping_source) -file(READ "${POINTLIO_SOURCE_DIR}/include/ivox/ivox3d.h" pointlio_ivox_source) -string(FIND "${pointlio_mapping_source}" "crossmat_list.reserve" pointlio_has_reserve) -string(FIND "${pointlio_mapping_source}" "crossmat_list.resize" pointlio_has_resize) -string(FIND "${pointlio_ivox_source}" "#include " pointlio_has_glog) - -if(NOT pointlio_has_reserve EQUAL -1 AND NOT pointlio_has_glog EQUAL -1) - set(POINTLIO_PATCHED_DIR "${CMAKE_BINARY_DIR}/pointlio-patched") - file(REMOVE_RECURSE "${POINTLIO_PATCHED_DIR}") - file(MAKE_DIRECTORY "${POINTLIO_PATCHED_DIR}") - file(COPY "${POINTLIO_SOURCE_DIR}/" DESTINATION "${POINTLIO_PATCHED_DIR}") - find_package(Git REQUIRED) - execute_process( - COMMAND "${GIT_EXECUTABLE}" apply "${CMAKE_CURRENT_SOURCE_DIR}/pointlio-gos.patch" - WORKING_DIRECTORY "${POINTLIO_PATCHED_DIR}" - RESULT_VARIABLE pointlio_patch_result - ERROR_VARIABLE pointlio_patch_error - ) - if(NOT pointlio_patch_result EQUAL 0) - message(FATAL_ERROR "Could not patch the pinned Point-LIO source: ${pointlio_patch_error}") - endif() - set(POINTLIO_DIR "${POINTLIO_PATCHED_DIR}") -elseif(NOT pointlio_has_resize EQUAL -1 AND pointlio_has_glog EQUAL -1) - message(STATUS "Using an already-patched Point-LIO source: ${POINTLIO_SOURCE_DIR}") -else() - message(FATAL_ERROR "Point-LIO source is neither the pinned original nor the expected patched tree") -endif() - -if(NOT DEFINED DIMOS_NATIVE_CPP_DIR) - set(DIMOS_NATIVE_CPP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../../native/cpp) -endif() -add_subdirectory(${DIMOS_NATIVE_CPP_DIR} ${CMAKE_BINARY_DIR}/dimos_native) - -add_executable(m20_pointlio - main.cpp - ${POINTLIO_DIR}/src/preprocess.cpp - ${POINTLIO_DIR}/src/Estimator.cpp - ${POINTLIO_DIR}/src/parameters.cpp -) -target_include_directories(m20_pointlio PRIVATE - ${POINTLIO_DIR}/include - ${POINTLIO_DIR}/include/IKFoM/IKFoM_toolkit - ${POINTLIO_DIR}/src - ${dimos_lcm_SOURCE_DIR}/generated/cpp_lcm_msgs - ${PCL_INCLUDE_DIRS} - ${CMAKE_CURRENT_SOURCE_DIR}/../../../../../hardware/sensors/lidar/common -) -target_compile_definitions(m20_pointlio PRIVATE - MP_PROC_NUM=1 - ROOT_DIR="/tmp/m20_pointlio_" -) -target_compile_options(m20_pointlio PRIVATE -Wall -Wextra -Wpedantic) -target_link_libraries(m20_pointlio - dimos_native - Eigen3::Eigen - PkgConfig::LCM - ${PCL_LIBRARIES} -) -ament_target_dependencies(m20_pointlio - rclcpp - sensor_msgs -) -if(OpenMP_CXX_FOUND) - target_link_libraries(m20_pointlio OpenMP::OpenMP_CXX) -endif() diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh b/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh deleted file mode 100755 index b90c5525b9..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/build.sh +++ /dev/null @@ -1,44 +0,0 @@ -#!/usr/bin/env bash -# Copyright 2026 Dimensional Inc. -# SPDX-License-Identifier: Apache-2.0 - -set -euo pipefail - -pointlio_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" -ros_setup="${M20_ROS_SETUP:-/opt/robot/scripts/setup_ros2.sh}" - -if [[ -f "$ros_setup" ]]; then - # shellcheck disable=SC1090 - set +u - source "$ros_setup" - set -u -elif [[ -f /opt/ros/foxy/setup.bash ]]; then - # shellcheck disable=SC1091 - set +u - source /opt/ros/foxy/setup.bash - set -u -else - echo "M20 ROS setup not found: $ros_setup" >&2 - exit 1 -fi - -cmake_args=( - -S "$pointlio_dir" - -B "$pointlio_dir/build" - -DCMAKE_BUILD_TYPE=Release -) -if [[ -n "${DIMOS_LCM_DIR:-}" ]]; then - cmake_args+=("-DDIMOS_LCM_DIR=${DIMOS_LCM_DIR}") -fi -if [[ -n "${M20_POINTLIO_DIR:-}" ]]; then - cmake_args+=("-DPOINTLIO_DIR=${M20_POINTLIO_DIR}") -fi -if [[ -n "${M20_PFR_DIR:-}" ]]; then - cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_PFR=${M20_PFR_DIR}") -fi -if [[ -n "${M20_NLOHMANN_JSON_DIR:-}" ]]; then - cmake_args+=("-DFETCHCONTENT_SOURCE_DIR_NLOHMANN_JSON=${M20_NLOHMANN_JSON_DIR}") -fi - -cmake "${cmake_args[@]}" -cmake --build "$pointlio_dir/build" --parallel "${M20_BUILD_JOBS:-4}" diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp b/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp deleted file mode 100644 index be86b48c50..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/main.cpp +++ /dev/null @@ -1,802 +0,0 @@ -// Copyright 2026 Dimensional Inc. -// SPDX-License-Identifier: Apache-2.0 -// -// M20 Point-LIO adapter. This process subscribes directly to the robot's public -// merged RoboSense PointCloud2 and base-aligned IMU over ROS 2, converts them -// into the pinned Point-LIO core, and owns odom -> base_link. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "dimos/native.hpp" - -#include "geometry_msgs/PoseStamped.hpp" -#include "geometry_msgs/TransformStamped.hpp" -#include "sensor_msgs/Imu.hpp" -#include "sensor_msgs/PointCloud2.hpp" -#include "sensor_msgs/PointField.hpp" -#include "tf2_msgs/TFMessage.hpp" - -#include "estimator_pose.hpp" -#include "point_cloud_utils.hpp" - -// Existing DimOS Point-LIO core, pinned by CMake. -#include "pointlio.hpp" -#include "pointlio_debug.hpp" - -using dimos::native::Builder; -using dimos::native::Config; -using dimos::native::Module; -using dimos::native::Output; -namespace logging = dimos::native::log; - -namespace { - -using Clock = std::chrono::steady_clock; -constexpr double kStandardGravityMps2 = 9.80665; -constexpr std::size_t kM20RawPointLimit = 500'000; -constexpr std::size_t kMaxInitializationLidarFrames = 20; - -void require_nonempty(const std::string& value, const char* name) { - if (value.empty()) { - throw std::runtime_error(std::string(name) + " must not be empty"); - } -} - -void require_vector_size(const std::vector& value, std::size_t expected, - const char* name) { - if (value.size() != expected) { - throw std::runtime_error(std::string(name) + " must contain " + - std::to_string(expected) + " values"); - } -} - -int ivox_nearby_code(const std::string& name) { - if (name == "center") return 0; - if (name == "nearby6") return 6; - if (name == "nearby18") return 18; - if (name == "nearby26") return 26; - throw std::runtime_error( - "ivox_nearby_type must be one of: center nearby6 nearby18 nearby26, got '" + - name + "'"); -} - -double header_seconds(const std_msgs::msg::Header& header) { - return static_cast(header.stamp.sec) + - static_cast(header.stamp.nanosec) / 1e9; -} - -template -T read_unaligned(const std::vector& data, std::size_t offset) { - T result{}; - std::memcpy(&result, data.data() + offset, sizeof(T)); - return result; -} - -struct M20FieldOffsets { - std::size_t x; - std::size_t y; - std::size_t z; - std::size_t intensity; - std::size_t ring; - std::size_t timestamp; -}; - -M20FieldOffsets validate_m20_cloud(const sensor_msgs::msg::PointCloud2& cloud) { - if (cloud.height == 0 || cloud.width == 0) { - throw std::runtime_error("M20 point cloud is empty"); - } - if (cloud.is_bigendian) { - throw std::runtime_error("M20 Point-LIO requires a little-endian cloud"); - } - if (cloud.point_step == 0 || cloud.row_step == 0) { - throw std::runtime_error("M20 point cloud has an invalid stride"); - } - const auto point_count = static_cast(cloud.width) * - static_cast(cloud.height); - if (point_count > kM20RawPointLimit) { - throw std::runtime_error("M20 point cloud exceeds the raw input sanity limit"); - } - const auto required_bytes = point_count * static_cast(cloud.point_step); - if (required_bytes > cloud.data.size()) { - throw std::runtime_error("M20 point cloud data is shorter than its dimensions"); - } - - constexpr auto missing = std::numeric_limits::max(); - M20FieldOffsets offsets{missing, missing, missing, missing, missing, missing}; - for (const auto& field : cloud.fields) { - const auto offset = static_cast(field.offset); - if (field.count == 0 || offset >= static_cast(cloud.point_step)) { - continue; - } - if (field.name == "x" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) { - offsets.x = offset; - } else if (field.name == "y" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) { - offsets.y = offset; - } else if (field.name == "z" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) { - offsets.z = offset; - } else if (field.name == "intensity" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) { - offsets.intensity = offset; - } else if (field.name == "ring" && - field.datatype == sensor_msgs::msg::PointField::UINT16) { - offsets.ring = offset; - } else if (field.name == "timestamp" && - field.datatype == sensor_msgs::msg::PointField::FLOAT64) { - offsets.timestamp = offset; - } - } - if (offsets.x == missing || offsets.y == missing || offsets.z == missing || - offsets.intensity == missing || offsets.ring == missing || - offsets.timestamp == missing) { - throw std::runtime_error( - "M20 Point-LIO requires float32 x/y/z/intensity, uint16 ring, and " - "float64 timestamp fields"); - } - if (offsets.timestamp + sizeof(double) > static_cast(cloud.point_step) || - offsets.ring + sizeof(uint16_t) > static_cast(cloud.point_step) || - offsets.intensity + sizeof(float) > static_cast(cloud.point_step)) { - throw std::runtime_error("M20 point fields extend past point_step"); - } - return offsets; -} - -struct TimedPoint { - double timestamp; - custom_messages::CustomPoint point; -}; - -} // namespace - -struct M20PointLioConfig { - std::string lidar_topic; - std::string imu_topic; - std::string node_name; - std::string world_frame; - std::string base_frame; - double pointcloud_rate_hz; - double odometry_rate_hz; - double max_scan_duration_s; - double msr_freq; - double main_freq; - bool con_frame; - int con_frame_num; - bool cut_frame; - double cut_frame_time_interval; - double time_lag_imu_to_lidar; - int scan_line; - int scan_rate; - double blind; - int point_filter_num; - bool use_imu_as_input; - bool prop_at_freq_of_imu; - bool check_satu; - int init_map_size; - bool space_down_sample; - double satu_acc; - double satu_gyro; - double acc_norm; - double plane_thr; - double filter_size_surf; - double filter_size_map; - double ivox_grid_resolution; - std::string ivox_nearby_type; - double cube_side_length; - double det_range; - double fov_degree; - bool imu_en; - bool start_in_aggressive_motion; - bool extrinsic_est_en; - double imu_time_inte; - double lidar_meas_cov; - double acc_cov_input; - double vel_cov; - double gyr_cov_input; - double gyr_cov_output; - double acc_cov_output; - double b_gyr_cov; - double b_acc_cov; - double imu_meas_acc_cov; - double imu_meas_omg_cov; - double match_s; - bool gravity_align; - std::vector gravity; - std::vector gravity_init; - std::vector extrinsic_t; - std::vector extrinsic_r; - bool publish_odometry_without_downsample; - bool odom_only; - bool debug; - - void validate() const { - require_nonempty(lidar_topic, "lidar_topic"); - require_nonempty(imu_topic, "imu_topic"); - require_nonempty(node_name, "node_name"); - require_nonempty(world_frame, "world_frame"); - require_nonempty(base_frame, "base_frame"); - dimos::native::require_positive(pointcloud_rate_hz, "pointcloud_rate_hz"); - dimos::native::require_positive(odometry_rate_hz, "odometry_rate_hz"); - dimos::native::require_positive(max_scan_duration_s, "max_scan_duration_s"); - dimos::native::require_positive(msr_freq, "msr_freq"); - dimos::native::require_positive(main_freq, "main_freq"); - if (scan_line <= 0 || scan_line > std::numeric_limits::max()) { - throw std::runtime_error("scan_line must be in [1, 65535]"); - } - if (point_filter_num <= 0 || init_map_size <= 0 || con_frame_num <= 0) { - throw std::runtime_error( - "point_filter_num, init_map_size, and con_frame_num must be positive"); - } - require_vector_size(gravity, 3, "gravity"); - require_vector_size(gravity_init, 3, "gravity_init"); - require_vector_size(extrinsic_t, 3, "extrinsic_t"); - require_vector_size(extrinsic_r, 9, "extrinsic_r"); - (void)ivox_nearby_code(ivox_nearby_type); - } -}; - -// GOS ships GCC 9. Its C++20 implementation is sufficient for the native SDK -// and Point-LIO, but not for Boost.PFR's compile-time field-name extraction. -// Parse keys explicitly so the source builds with the robot's stock toolchain -// while retaining strict unknown/missing-key validation. -M20PointLioConfig parse_m20_pointlio_config(Config& config) { - M20PointLioConfig result{}; - result.lidar_topic = config.take("lidar_topic"); - result.imu_topic = config.take("imu_topic"); - result.node_name = config.take("node_name"); - result.world_frame = config.take("world_frame"); - result.base_frame = config.take("base_frame"); - result.pointcloud_rate_hz = config.take("pointcloud_rate_hz"); - result.odometry_rate_hz = config.take("odometry_rate_hz"); - result.max_scan_duration_s = config.take("max_scan_duration_s"); - result.msr_freq = config.take("msr_freq"); - result.main_freq = config.take("main_freq"); - result.con_frame = config.take("con_frame"); - result.con_frame_num = config.take("con_frame_num"); - result.cut_frame = config.take("cut_frame"); - result.cut_frame_time_interval = config.take("cut_frame_time_interval"); - result.time_lag_imu_to_lidar = config.take("time_lag_imu_to_lidar"); - result.scan_line = config.take("scan_line"); - result.scan_rate = config.take("scan_rate"); - result.blind = config.take("blind"); - result.point_filter_num = config.take("point_filter_num"); - result.use_imu_as_input = config.take("use_imu_as_input"); - result.prop_at_freq_of_imu = config.take("prop_at_freq_of_imu"); - result.check_satu = config.take("check_satu"); - result.init_map_size = config.take("init_map_size"); - result.space_down_sample = config.take("space_down_sample"); - result.satu_acc = config.take("satu_acc"); - result.satu_gyro = config.take("satu_gyro"); - result.acc_norm = config.take("acc_norm"); - result.plane_thr = config.take("plane_thr"); - result.filter_size_surf = config.take("filter_size_surf"); - result.filter_size_map = config.take("filter_size_map"); - result.ivox_grid_resolution = config.take("ivox_grid_resolution"); - result.ivox_nearby_type = config.take("ivox_nearby_type"); - result.cube_side_length = config.take("cube_side_length"); - result.det_range = config.take("det_range"); - result.fov_degree = config.take("fov_degree"); - result.imu_en = config.take("imu_en"); - result.start_in_aggressive_motion = config.take("start_in_aggressive_motion"); - result.extrinsic_est_en = config.take("extrinsic_est_en"); - result.imu_time_inte = config.take("imu_time_inte"); - result.lidar_meas_cov = config.take("lidar_meas_cov"); - result.acc_cov_input = config.take("acc_cov_input"); - result.vel_cov = config.take("vel_cov"); - result.gyr_cov_input = config.take("gyr_cov_input"); - result.gyr_cov_output = config.take("gyr_cov_output"); - result.acc_cov_output = config.take("acc_cov_output"); - result.b_gyr_cov = config.take("b_gyr_cov"); - result.b_acc_cov = config.take("b_acc_cov"); - result.imu_meas_acc_cov = config.take("imu_meas_acc_cov"); - result.imu_meas_omg_cov = config.take("imu_meas_omg_cov"); - result.match_s = config.take("match_s"); - result.gravity_align = config.take("gravity_align"); - result.gravity = config.take>("gravity"); - result.gravity_init = config.take>("gravity_init"); - result.extrinsic_t = config.take>("extrinsic_t"); - result.extrinsic_r = config.take>("extrinsic_r"); - result.publish_odometry_without_downsample = - config.take("publish_odometry_without_downsample"); - result.odom_only = config.take("odom_only"); - result.debug = config.take("debug"); - config.enforce_all_consumed(); - result.validate(); - return result; -} - -class M20PointLio : public Module { -public: - void build(Builder& builder, Config& config) override { - cfg_ = parse_m20_pointlio_config(config); - - lidar_ = builder.output("lidar"); - odom_ = builder.output("odom"); - tf_ = builder.output("tf"); - - process_period_ = std::chrono::duration_cast( - std::chrono::duration(1.0 / cfg_.main_freq)); - pointcloud_period_ = std::chrono::duration_cast( - std::chrono::duration(1.0 / cfg_.pointcloud_rate_hz)); - odometry_period_ = std::chrono::duration_cast( - std::chrono::duration(1.0 / cfg_.odometry_rate_hz)); - } - - void setup() override { - pointlio_debug = cfg_.debug; - - PointLioParams params; - params.odom_header_frame_id = cfg_.world_frame; - params.odom_child_frame_id = cfg_.base_frame; - params.con_frame = cfg_.con_frame; - params.con_frame_num = cfg_.con_frame_num; - params.cut_frame = cfg_.cut_frame; - params.cut_frame_time_interval = cfg_.cut_frame_time_interval; - params.time_lag_imu_to_lidar = cfg_.time_lag_imu_to_lidar; - // The M20 converter emits the same timestamped CustomMsg shape as the - // existing Mid-360 adapter, so the core intentionally stays in AVIA mode. - params.lidar_type = 1; - params.scan_line = cfg_.scan_line; - params.scan_rate = cfg_.scan_rate; - params.timestamp_unit = 3; - params.blind = cfg_.blind; - params.point_filter_num = cfg_.point_filter_num; - params.use_imu_as_input = cfg_.use_imu_as_input; - params.prop_at_freq_of_imu = cfg_.prop_at_freq_of_imu; - params.check_satu = cfg_.check_satu; - params.init_map_size = cfg_.init_map_size; - params.space_down_sample = cfg_.space_down_sample; - params.satu_acc = cfg_.satu_acc; - params.satu_gyro = cfg_.satu_gyro; - params.acc_norm = cfg_.acc_norm; - params.plane_thr = static_cast(cfg_.plane_thr); - params.filter_size_surf = cfg_.filter_size_surf; - params.filter_size_map = cfg_.filter_size_map; - params.ivox_grid_resolution = static_cast(cfg_.ivox_grid_resolution); - params.ivox_nearby_type = ivox_nearby_code(cfg_.ivox_nearby_type); - params.cube_side_length = cfg_.cube_side_length; - params.det_range = static_cast(cfg_.det_range); - params.fov_degree = cfg_.fov_degree; - params.imu_en = cfg_.imu_en; - params.start_in_aggressive_motion = cfg_.start_in_aggressive_motion; - params.extrinsic_est_en = cfg_.extrinsic_est_en; - params.imu_time_inte = cfg_.imu_time_inte; - params.lidar_meas_cov = cfg_.lidar_meas_cov; - params.acc_cov_input = cfg_.acc_cov_input; - params.vel_cov = cfg_.vel_cov; - params.gyr_cov_input = cfg_.gyr_cov_input; - params.gyr_cov_output = cfg_.gyr_cov_output; - params.acc_cov_output = cfg_.acc_cov_output; - params.b_gyr_cov = cfg_.b_gyr_cov; - params.b_acc_cov = cfg_.b_acc_cov; - params.imu_meas_acc_cov = cfg_.imu_meas_acc_cov; - params.imu_meas_omg_cov = cfg_.imu_meas_omg_cov; - params.match_s = cfg_.match_s; - params.gravity_align = cfg_.gravity_align; - params.gravity = cfg_.gravity; - params.gravity_init = cfg_.gravity_init; - params.extrinsic_T = cfg_.extrinsic_t; - params.extrinsic_R = cfg_.extrinsic_r; - params.publish_odometry_without_downsample = - cfg_.publish_odometry_without_downsample; - params.odom_only = cfg_.odom_only; - - point_lio_ = std::make_unique(params, cfg_.msr_freq, cfg_.main_freq); - - rclcpp::init(0, nullptr); - dimos::native::install_signal_handlers(); - node_ = std::make_shared(cfg_.node_name); - lidar_callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - imu_callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); - - rclcpp::SubscriptionOptions lidar_options; - lidar_options.callback_group = lidar_callback_group_; - rclcpp::SubscriptionOptions imu_options; - imu_options.callback_group = imu_callback_group_; - const auto lidar_qos = - rclcpp::QoS(rclcpp::KeepLast(2)).reliable().durability_volatile(); - const auto imu_qos = - rclcpp::QoS(rclcpp::KeepLast(20)).reliable().durability_volatile(); - lidar_subscription_ = node_->create_subscription( - cfg_.lidar_topic, lidar_qos, - [this](sensor_msgs::msg::PointCloud2::SharedPtr message) { - on_lidar(*message); - }, - lidar_options); - imu_subscription_ = node_->create_subscription( - cfg_.imu_topic, imu_qos, - [this](sensor_msgs::msg::Imu::SharedPtr message) { on_imu(*message); }, - imu_options); - executor_ = std::make_shared( - rclcpp::ExecutorOptions(), 2); - executor_->add_node(node_); - - const auto now = Clock::now(); - last_pointcloud_publish_ = now; - last_odometry_publish_ = now; - spin_thread_ = std::thread([this]() { executor_->spin(); }); - logging::info("M20 Point-LIO started", - {logging::Field("world_frame", cfg_.world_frame), - logging::Field("base_frame", cfg_.base_frame), - logging::Field("scan_lines", static_cast(cfg_.scan_line)), - logging::Field("lidar_topic", cfg_.lidar_topic), - logging::Field("imu_topic", cfg_.imu_topic)}); - } - - void teardown() override { - stopping_.store(true, std::memory_order_release); - if (executor_ != nullptr) { - executor_->cancel(); - } - if (spin_thread_.joinable()) { - spin_thread_.join(); - } - lidar_subscription_.reset(); - imu_subscription_.reset(); - lidar_callback_group_.reset(); - imu_callback_group_.reset(); - executor_.reset(); - node_.reset(); - if (rclcpp::ok()) { - rclcpp::shutdown(); - } - point_lio_.reset(); - } - - void handle() override { - while (!shutdown_requested()) { - const auto iteration_started = Clock::now(); - bool have_estimate = false; - double estimate_stamp = 0.0; - point_lio_->process(); - const auto pose = point_lio_->get_pose(); - have_estimate = dimos::has_estimate(pose); - if (have_estimate) { - const auto& source_odom = point_lio_->get_odometry(); - estimate_stamp = source_odom.header.stamp.toSec(); - const auto now = Clock::now(); - if (std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { - if (now - last_pointcloud_publish_ >= pointcloud_period_ && - estimate_stamp > last_pointcloud_stamp_) { - const auto cloud = point_lio_->get_body_cloud(); - if (cloud != nullptr && !cloud->empty()) { - publish_pointcloud(cloud, estimate_stamp); - last_pointcloud_stamp_ = estimate_stamp; - last_pointcloud_publish_ = now; - } - } - if (now - last_odometry_publish_ >= odometry_period_ && - estimate_stamp > last_odometry_stamp_) { - publish_odometry(source_odom, estimate_stamp); - last_odometry_stamp_ = estimate_stamp; - last_odometry_publish_ = now; - } - } - } - - bool estimate_advanced = false; - if (have_estimate && std::isfinite(estimate_stamp) && estimate_stamp > 0.0) { - if (estimate_stamp > last_processed_estimate_stamp_) { - last_processed_estimate_stamp_ = estimate_stamp; - estimate_advanced = true; - } - } - if (estimate_advanced) { - std::lock_guard lock(lidar_feed_mutex_); - estimator_initialized_ = true; - lidar_feed_pending_ = false; - } - - const auto elapsed = Clock::now() - iteration_started; - if (elapsed < process_period_) { - std::this_thread::sleep_for(process_period_ - elapsed); - } - } - } - -private: - void on_lidar(const sensor_msgs::msg::PointCloud2& source) { - if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; - bool feed_reserved = false; - - try { - const auto offsets = validate_m20_cloud(source); - - std::unique_lock callback_lock(lidar_callback_mutex_, - std::try_to_lock); - if (!callback_lock.owns_lock() || point_lio_is_processing_lidar()) { - log_busy_lidar_drop(); - return; - } - - const auto point_count = static_cast(source.width) * - static_cast(source.height); - const auto point_step = static_cast(source.point_step); - std::vector points; - points.reserve(point_count); - uint16_t min_ring = std::numeric_limits::max(); - uint16_t max_ring = 0; - - for (std::size_t index = 0; index < point_count; ++index) { - const auto base = index * point_step; - const float x = read_unaligned(source.data, base + offsets.x); - const float y = read_unaligned(source.data, base + offsets.y); - const float z = read_unaligned(source.data, base + offsets.z); - const float intensity = - read_unaligned(source.data, base + offsets.intensity); - const uint16_t ring = - read_unaligned(source.data, base + offsets.ring); - const double timestamp = - read_unaligned(source.data, base + offsets.timestamp); - if (!std::isfinite(x) || !std::isfinite(y) || !std::isfinite(z) || - !std::isfinite(timestamp) || ring >= cfg_.scan_line) { - continue; - } - - custom_messages::CustomPoint point{}; - point.x = x; - point.y = y; - point.z = z; - point.reflectivity = static_cast(std::clamp( - std::isfinite(intensity) ? static_cast(intensity) : 0.0, 0.0, - 255.0)); - point.tag = 0; - point.line = ring; - points.push_back({timestamp, point}); - min_ring = std::min(min_ring, ring); - max_ring = std::max(max_ring, ring); - } - if (points.empty()) { - throw std::runtime_error("M20 cloud has no finite Point-LIO returns"); - } - - std::sort(points.begin(), points.end(), - [](const TimedPoint& left, const TimedPoint& right) { - return left.timestamp < right.timestamp; - }); - const double first_point_time = points.front().timestamp; - const double last_point_time = points.back().timestamp; - const double scan_duration = last_point_time - first_point_time; - if (scan_duration < 0.0 || scan_duration > cfg_.max_scan_duration_s) { - throw std::runtime_error("M20 per-point timestamp span is outside the " - "configured scan-duration limit"); - } - - const double source_header_time = header_seconds(source.header); - // rsdriver uses absolute PTP seconds today. Accept a relative - // per-scan timestamp too, but anchor that explicitly to the header. - const bool absolute_point_time = first_point_time > 100'000'000.0; - const double frame_time = absolute_point_time - ? first_point_time - : source_header_time + first_point_time; - if (!std::isfinite(frame_time) || frame_time <= 0.0) { - throw std::runtime_error("M20 cloud has no usable sensor timestamp"); - } - if (last_lidar_sensor_time_ > 0.0 && frame_time <= last_lidar_sensor_time_) { - logging::warn("dropping non-monotonic M20 lidar frame", - {logging::Field("stamp", frame_time), - logging::Field("previous_stamp", last_lidar_sensor_time_)}); - return; - } - - auto message = boost::make_shared(); - message->header.seq = 0; - message->header.stamp = custom_messages::Time().fromSec(frame_time); - message->header.frame_id = cfg_.base_frame; - message->timebase = static_cast(std::llround(frame_time * 1e9)); - message->lidar_id = 0; - for (auto& reserved : message->rsvd) reserved = 0; - message->points.reserve(points.size()); - for (auto& timed : points) { - const double offset_ns = (timed.timestamp - first_point_time) * 1e9; - timed.point.offset_time = - static_cast(std::max(0.0, std::round(offset_ns))); - message->points.push_back(timed.point); - } - message->point_num = static_cast(message->points.size()); - - // Point-LIO's feeder callbacks take its internal buffer mutex and - // are designed to run concurrently with process(). An outer lock - // here would block sensor ingestion for the full estimator step. - { - std::lock_guard lock(lidar_feed_mutex_); - if (estimator_initialized_) { - lidar_feed_pending_ = true; - } else { - ++initialization_lidar_frames_; - } - feed_reserved = true; - } - point_lio_->feed_lidar(message); - last_lidar_sensor_time_ = frame_time; - - if (!logged_cloud_contract_.exchange(true, std::memory_order_acq_rel)) { - logging::info( - "M20 Point-LIO accepted cloud contract", - {logging::Field("input_points", static_cast(point_count)), - logging::Field("valid_points", - static_cast(points.size())), - logging::Field("min_ring", static_cast(min_ring)), - logging::Field("max_ring", static_cast(max_ring)), - logging::Field("scan_duration_s", scan_duration), - logging::Field("absolute_point_time", absolute_point_time)}); - } - } catch (const std::exception& error) { - if (feed_reserved) { - std::lock_guard lock(lidar_feed_mutex_); - if (estimator_initialized_) { - lidar_feed_pending_ = false; - } else if (initialization_lidar_frames_ > 0) { - --initialization_lidar_frames_; - } - } - logging::error("dropping M20 cloud before Point-LIO", - {logging::Field("error", std::string(error.what()))}); - } - } - - bool point_lio_is_processing_lidar() const { - std::lock_guard lock(lidar_feed_mutex_); - if (!estimator_initialized_) { - return initialization_lidar_frames_ >= kMaxInitializationLidarFrames; - } - return lidar_feed_pending_; - } - - void log_busy_lidar_drop() { - const auto dropped = - busy_lidar_drops_.fetch_add(1, std::memory_order_acq_rel) + 1; - if (dropped == 1 || dropped % 500 == 0) { - logging::info( - "shedding M20 lidar frame while Point-LIO processes the previous frame", - {logging::Field("dropped", static_cast(dropped))}); - } - } - - void on_imu(const sensor_msgs::msg::Imu& source) { - if (stopping_.load(std::memory_order_acquire) || point_lio_ == nullptr) return; - const double timestamp = header_seconds(source.header); - if (!std::isfinite(timestamp) || timestamp <= 0.0 || - !std::isfinite(source.angular_velocity.x) || - !std::isfinite(source.angular_velocity.y) || - !std::isfinite(source.angular_velocity.z) || - !std::isfinite(source.linear_acceleration.x) || - !std::isfinite(source.linear_acceleration.y) || - !std::isfinite(source.linear_acceleration.z)) { - logging::error("dropping invalid M20 IMU sample"); - return; - } - if (last_imu_sensor_time_ > 0.0 && timestamp <= last_imu_sensor_time_) { - logging::warn("dropping non-monotonic M20 IMU sample", - {logging::Field("stamp", timestamp), - logging::Field("previous_stamp", last_imu_sensor_time_)}); - return; - } - - auto message = boost::make_shared(); - message->header.seq = 0; - message->header.stamp = custom_messages::Time().fromSec(timestamp); - message->header.frame_id = cfg_.base_frame; - message->orientation.x = source.orientation.x; - message->orientation.y = source.orientation.y; - message->orientation.z = source.orientation.z; - message->orientation.w = source.orientation.w; - message->angular_velocity.x = source.angular_velocity.x; - message->angular_velocity.y = source.angular_velocity.y; - message->angular_velocity.z = source.angular_velocity.z; - // ROS sensor_msgs/Imu is m/s^2; this Point-LIO core expects g. - message->linear_acceleration.x = source.linear_acceleration.x / kStandardGravityMps2; - message->linear_acceleration.y = source.linear_acceleration.y / kStandardGravityMps2; - message->linear_acceleration.z = source.linear_acceleration.z / kStandardGravityMps2; - for (int index = 0; index < 9; ++index) { - message->orientation_covariance[index] = source.orientation_covariance[index]; - message->angular_velocity_covariance[index] = - source.angular_velocity_covariance[index]; - message->linear_acceleration_covariance[index] = - source.linear_acceleration_covariance[index] / - (kStandardGravityMps2 * kStandardGravityMps2); - } - - point_lio_->feed_imu(message); - last_imu_sensor_time_ = timestamp; - } - - void publish_pointcloud(const PointCloudXYZI::Ptr& cloud, double timestamp) { - const auto count = static_cast(cloud->size()); - auto output = dimos::make_xyzi_cloud(cfg_.base_frame, timestamp, count); - for (int index = 0; index < count; ++index) { - float* point = dimos::xyzi_point(output, index); - point[0] = cloud->points[index].x; - point[1] = cloud->points[index].y; - point[2] = cloud->points[index].z; - point[3] = cloud->points[index].intensity; - } - lidar_.publish(output); - } - - void publish_odometry(const custom_messages::Odometry& source, double timestamp) { - geometry_msgs::PoseStamped pose; - pose.header = dimos::make_header(cfg_.world_frame, timestamp); - pose.pose.position.x = source.pose.pose.position.x; - pose.pose.position.y = source.pose.pose.position.y; - pose.pose.position.z = source.pose.pose.position.z; - pose.pose.orientation.x = source.pose.pose.orientation.x; - pose.pose.orientation.y = source.pose.pose.orientation.y; - pose.pose.orientation.z = source.pose.pose.orientation.z; - pose.pose.orientation.w = source.pose.pose.orientation.w; - - geometry_msgs::TransformStamped transform; - transform.header = pose.header; - transform.child_frame_id = cfg_.base_frame; - transform.transform.translation.x = pose.pose.position.x; - transform.transform.translation.y = pose.pose.position.y; - transform.transform.translation.z = pose.pose.position.z; - transform.transform.rotation = pose.pose.orientation; - tf2_msgs::TFMessage transforms; - transforms.transforms_length = 1; - transforms.transforms.push_back(std::move(transform)); - - odom_.publish(pose); - tf_.publish(transforms); - } - - M20PointLioConfig cfg_; - Output lidar_; - Output odom_; - Output tf_; - std::unique_ptr point_lio_; - - std::shared_ptr node_; - std::shared_ptr executor_; - rclcpp::CallbackGroup::SharedPtr lidar_callback_group_; - rclcpp::CallbackGroup::SharedPtr imu_callback_group_; - rclcpp::Subscription::SharedPtr lidar_subscription_; - rclcpp::Subscription::SharedPtr imu_subscription_; - std::thread spin_thread_; - - Clock::duration process_period_{}; - Clock::duration pointcloud_period_{}; - Clock::duration odometry_period_{}; - Clock::time_point last_pointcloud_publish_{}; - Clock::time_point last_odometry_publish_{}; - double last_pointcloud_stamp_ = 0.0; - double last_odometry_stamp_ = 0.0; - double last_lidar_sensor_time_ = 0.0; - double last_imu_sensor_time_ = 0.0; - double last_processed_estimate_stamp_ = 0.0; - std::atomic stopping_{false}; - mutable std::mutex lidar_feed_mutex_; - std::mutex lidar_callback_mutex_; - std::size_t initialization_lidar_frames_ = 0; - bool estimator_initialized_ = false; - bool lidar_feed_pending_ = false; - std::atomic busy_lidar_drops_{0}; - std::atomic logged_cloud_contract_{false}; -}; - -int main() { - dimos::native::run_with_transport(); - return 0; -} diff --git a/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch b/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch deleted file mode 100644 index 6309fb0a4e..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/cpp/pointlio-gos.patch +++ /dev/null @@ -1,16 +0,0 @@ -Point-LIO fixes required by the M20 GOS build. - ---- a/src/laserMapping.hpp -+++ b/src/laserMapping.hpp -@@ -644,4 +644,4 @@ - /*** iterated state estimation ***/ -- crossmat_list.reserve(feats_down_size); -- pbody_list.reserve(feats_down_size); -+ crossmat_list.resize(feats_down_size); -+ pbody_list.resize(feats_down_size); - // pbody_ext_list.reserve(feats_down_size); ---- a/include/ivox/ivox3d.h -+++ b/include/ivox/ivox3d.h -@@ -8,2 +8 @@ --#include - // #include diff --git a/dimos/robot/deeprobotics/m20/pointlio/module.py b/dimos/robot/deeprobotics/m20/pointlio/module.py deleted file mode 100644 index 741e0de1fe..0000000000 --- a/dimos/robot/deeprobotics/m20/pointlio/module.py +++ /dev/null @@ -1,141 +0,0 @@ -# Copyright 2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Native Point-LIO wrapper with direct M20 ROS lidar and IMU ingress.""" - -from __future__ import annotations - -from typing import TYPE_CHECKING, Literal - -from pydantic import Field - -from dimos.core.native_module import NativeModule, NativeModuleConfig -from dimos.core.stream import Out -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.spec import perception - -IvoxNearbyType = Literal["center", "nearby6", "nearby18", "nearby26"] - - -class M20PointLioConfig(NativeModuleConfig): - """M20 Point-LIO input contract and robot-specific estimator tuning.""" - - cwd: str | None = "cpp" - executable: str = "build/m20_pointlio" - build_command: str | None = "./build.sh" - stdin_config: bool = True - extra_env: dict[str, str] = Field( - default_factory=lambda: { - "FASTRTPS_DEFAULT_PROFILES_FILE": "/opt/robot/fastdds.xml", - "LD_LIBRARY_PATH": "/opt/ros/foxy/lib", - "RMW_IMPLEMENTATION": "rmw_fastrtps_cpp", - } - ) - # GOS isolates its RK3588 big cores. Cores 6-7 run the vendor lidar - # drivers, core 5 is assigned to mapping, and Point-LIO owns core 4. - cpu_affinity: frozenset[int] | None = frozenset({4}) - - lidar_topic: str = "/LIDAR/POINTS" - imu_topic: str = "/IMU" - node_name: str = "dimos_m20_pointlio" - world_frame: str = "odom" - base_frame: str = "base_link" - pointcloud_rate_hz: float = Field(default=10.0, gt=0.0) - odometry_rate_hz: float = Field(default=50.0, gt=0.0) - max_scan_duration_s: float = Field(default=0.2, gt=0.0) - - msr_freq: float = Field(default=200.0, gt=0.0) - main_freq: float = Field(default=1000.0, gt=0.0) - con_frame: bool = False - con_frame_num: int = Field(default=1, gt=0) - cut_frame: bool = False - cut_frame_time_interval: float = Field(default=0.1, gt=0.0) - time_lag_imu_to_lidar: float = 0.0 - # The vendor merged cloud offsets the second lidar's rings: front uses - # 0-95 and rear uses 96-191. Treating this as a single 96-line lidar drops - # the complete rear scan in the native adapter before Point-LIO sees it. - scan_line: int = Field(default=192, gt=0) - scan_rate: int = Field(default=10, gt=0) - blind: float = Field(default=0.5, ge=0.0) - # Use Point-LIO's standard pre-KF decimator for the merged dual-lidar cloud. - # Queue growth is bounded separately by the adapter's one-frame admission gate. - point_filter_num: int = Field(default=8, gt=0) - - use_imu_as_input: bool = False - prop_at_freq_of_imu: bool = True - check_satu: bool = True - init_map_size: int = Field(default=10, gt=0) - space_down_sample: bool = True - satu_acc: float = Field(default=3.0, gt=0.0) - satu_gyro: float = Field(default=35.0, gt=0.0) - # Point-LIO expects acceleration in g. The native adapter converts the - # M20's ROS-standard m/s^2 values before feeding the estimator. - acc_norm: float = Field(default=1.0, gt=0.0) - plane_thr: float = Field(default=0.1, gt=0.0) - filter_size_surf: float = Field(default=0.2, gt=0.0) - filter_size_map: float = Field(default=0.5, gt=0.0) - ivox_grid_resolution: float = Field(default=2.0, gt=0.0) - ivox_nearby_type: IvoxNearbyType = "nearby6" - cube_side_length: float = Field(default=1000.0, gt=0.0) - det_range: float = Field(default=60.0, gt=0.0) - fov_degree: float = Field(default=360.0, gt=0.0, le=360.0) - imu_en: bool = True - start_in_aggressive_motion: bool = False - extrinsic_est_en: bool = False - imu_time_inte: float = Field(default=0.005, gt=0.0) - lidar_meas_cov: float = Field(default=0.01, gt=0.0) - acc_cov_input: float = Field(default=0.1, gt=0.0) - vel_cov: float = Field(default=20.0, gt=0.0) - gyr_cov_input: float = Field(default=0.01, gt=0.0) - gyr_cov_output: float = Field(default=1000.0, gt=0.0) - acc_cov_output: float = Field(default=500.0, gt=0.0) - b_gyr_cov: float = Field(default=0.0001, gt=0.0) - b_acc_cov: float = Field(default=0.0001, gt=0.0) - imu_meas_acc_cov: float = Field(default=0.01, gt=0.0) - imu_meas_omg_cov: float = Field(default=0.01, gt=0.0) - match_s: float = Field(default=81.0, gt=0.0) - gravity_align: bool = True - gravity: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) - gravity_init: list[float] = Field(default_factory=lambda: [0.0, 0.0, -9.81]) - # Both public M20 streams are already expressed in base_link. Identity is - # therefore intentional: Point-LIO owns odom -> base_link with no hidden - # sensor/body transform. - extrinsic_t: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0]) - extrinsic_r: list[float] = Field( - default_factory=lambda: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - ) - publish_odometry_without_downsample: bool = False - odom_only: bool = False - debug: bool = False - - -class M20PointLio(NativeModule, perception.Lidar): - """Run the pinned Point-LIO core directly on the M20's ROS sensor topics. - - The native process subscribes to the merged ``base_link`` cloud and 200 Hz - ``base_link`` IMU itself, avoiding a full-payload LCM hop through the command - bridge. It has no vendor odometry input and owns ``odom -> base_link``. - """ - - config: M20PointLioConfig - - lidar: Out[PointCloud2] - odom: Out[PoseStamped] - tf: Out[TFMessage] - - -if TYPE_CHECKING: - M20PointLio() diff --git a/native/cpp/include/dimos/native/config.hpp b/native/cpp/include/dimos/native/config.hpp index c1d42068d9..74a5ad77f2 100644 --- a/native/cpp/include/dimos/native/config.hpp +++ b/native/cpp/include/dimos/native/config.hpp @@ -103,28 +103,6 @@ class Config { } } - /// Read and consume one required field without aggregate field-name reflection. - /// - /// Use this on deployment targets whose compiler supports the SDK runtime - /// but not the C++20 non-type template arguments required by - /// `pfr::names_as_array`. Call `enforce_all_consumed()` after the final - /// field to retain the same strict one-to-one config contract as parse(). - template - T take(const std::string& key) { - auto it = obj_.find(key); - if (it == obj_.end()) { - throw std::runtime_error("config: missing required field '" + key + "'"); - } - config_detail::check_json_type(*it, key); - try { - T value = it->template get(); - consumed_.insert(key); - return value; - } catch (const std::exception& e) { - throw std::runtime_error("config: field '" + key + "': " + e.what()); - } - } - /// Deserialize into a plain aggregate struct, enforcing the one-to-one key /// check (every field present, no unknowns) and the optional validate(). template @@ -136,7 +114,17 @@ class Config { constexpr auto names = pfr::names_as_array(); pfr::for_each_field(out, [&](auto& field, std::size_t i) { const std::string key(names[i]); - field = take>(key); + auto it = obj_.find(key); + if (it == obj_.end()) { + throw std::runtime_error("config: missing required field '" + key + "'"); + } + config_detail::check_json_type>(*it, key); + try { + field = it->template get>(); + } catch (const std::exception& e) { + throw std::runtime_error("config: field '" + key + "': " + e.what()); + } + consumed_.insert(key); }); enforce_all_consumed(); config_detail::validate_if_present(out); From 3cb66cc9c24c31012b9549c0ff01feec1158d6bf Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 2 Sep 2026 04:53:42 +0800 Subject: [PATCH 14/15] refactor(robot): send M20 cmd_vel directly --- dimos/robot/deeprobotics/m20/connection.py | 63 +++++-------------- .../onboard/drdds-zenoh-bridge/cpp/main.cpp | 8 +-- 2 files changed, 21 insertions(+), 50 deletions(-) diff --git a/dimos/robot/deeprobotics/m20/connection.py b/dimos/robot/deeprobotics/m20/connection.py index ad826b84d5..f820df24fc 100644 --- a/dimos/robot/deeprobotics/m20/connection.py +++ b/dimos/robot/deeprobotics/m20/connection.py @@ -78,26 +78,24 @@ class M20Connection(Module): """Expose the planner-facing M20 command and terrain surface. The hardware bridge owns ROS 2/DrDDS and the command watchdog. This module - remains transport-agnostic: it accepts the standard DimOS ``cmd_vel`` stream, - rejects it until ``standup()`` has enabled control, and emits ``safe_cmd_vel`` - for the robot-local bridge. The native bridge is the single command-validation + publishes manual movement RPCs on the standard DimOS ``cmd_vel`` stream. The + robot-local bridge consumes that stream and is the single command-validation and velocity-clamping boundary. ``standup()`` is the normal one-call operator entry point: it completes the - vendor state and gait transitions, waits for the guarded command path, and - enables velocity output. ``set_navigation_terrain()`` is the only exposed - vendor-specific control and selects one of the documented agile navigation - gaits without exposing raw gait values. + vendor state and gait transitions and waits for the command path. + ``set_navigation_terrain()`` is the only exposed vendor-specific control and + selects one of the documented agile navigation gaits without exposing raw + gait values. """ config: M20ConnectionConfig - cmd_vel: In[Twist] + cmd_vel: Out[Twist] command_ready: In[Bool] motion_state: In[Int32] gait_state: In[UInt32] odometry: In[Odometry] - safe_cmd_vel: Out[Twist] motion_state_cmd: Out[Int32] gait_cmd: Out[UInt32] odom: Out[PoseStamped] @@ -106,7 +104,6 @@ def __init__(self, **kwargs: Any) -> None: super().__init__(**kwargs) self._lock = RLock() self._state_condition = Condition(self._lock) - self._commands_enabled = False self._command_ready = False self._motion_state: int | None = None self._gait_state: int | None = None @@ -117,39 +114,36 @@ def __init__(self, **kwargs: Any) -> None: def start(self) -> None: super().start() self.register_disposable(Disposable(self.command_ready.subscribe(self._on_command_ready))) - self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) self.register_disposable(Disposable(self.motion_state.subscribe(self._on_motion_state))) self.register_disposable(Disposable(self.gait_state.subscribe(self._on_gait_state))) self.register_disposable(Disposable(self.odometry.subscribe(self._on_odometry))) - self.safe_cmd_vel.publish(Twist.zero()) @rpc def stop(self) -> None: - self._disable_commands() + self.stop_movement() super().stop() @rpc def move(self, twist: Twist, duration: float = 0.0) -> bool: - """Forward velocity to the native validation boundary when enabled. + """Publish velocity to the native validation boundary. ``duration`` is accepted for connection compatibility. Command lifetime is enforced by the native bridge's monotonic watchdog. """ del duration with self._lock: - enabled = self._commands_enabled and self._command_ready - self.safe_cmd_vel.publish(twist if enabled else Twist.zero()) - return enabled + ready = self._command_ready + self.cmd_vel.publish(twist) + return ready @rpc def stop_movement(self) -> None: """Publish an immediate zero velocity without disabling future commands.""" - self.safe_cmd_vel.publish(Twist.zero()) + self.cmd_vel.publish(Twist.zero()) @rpc def standup(self) -> bool: """Bring the M20 to a command-enabled, navigation-ready standing state.""" - self._disable_commands() if not self._enter_navigation_mode(): logger.error("M20 basic_server rejected the navigation usage mode") return False @@ -169,12 +163,12 @@ def standup(self) -> bool: if not self._wait_for_control_readiness(self.config.control_ready_timeout_s): logger.error("M20 robot control path did not become ready") return False - return self._enable_commands() + return True @rpc def liedown(self) -> bool: """Disable velocity output and command the M20 to its Sit/prone state.""" - self._disable_commands() + self.stop_movement() self.motion_state_cmd.publish(Int32(MOTION_SIT)) return True @@ -199,9 +193,8 @@ def set_navigation_terrain(self, terrain: NavigationTerrain) -> bool: if self._gait_state == gait: self._navigation_terrain = terrain return True - restore_commands = self._commands_enabled - self._disable_commands() + self.stop_movement() switched = self._set_gait_and_wait(gait) if switched: with self._lock: @@ -210,19 +203,12 @@ def set_navigation_terrain(self, terrain: NavigationTerrain) -> bool: else: logger.error("M20 did not confirm the agile %s navigation gait", terrain) - if restore_commands and not self._enable_commands(): - return False return switched def _on_command_ready(self, msg: Bool) -> None: - ready = bool(msg.data) with self._state_condition: - commands_were_enabled = self._commands_enabled - self._command_ready = ready + self._command_ready = bool(msg.data) self._state_condition.notify_all() - if commands_were_enabled and not ready: - self.safe_cmd_vel.publish(Twist.zero()) - logger.warning("M20 command output temporarily inhibited: robot control path is stale") def _on_motion_state(self, msg: Int32) -> None: with self._state_condition: @@ -273,21 +259,6 @@ def _enter_navigation_mode(self) -> bool: return False return True - def _enable_commands(self) -> bool: - with self._lock: - if not self._command_ready: - logger.warning("M20 command gate refused enable: robot control path is not ready") - return False - self._commands_enabled = True - logger.info("M20 command output enabled") - return True - - def _disable_commands(self) -> None: - with self._lock: - self._commands_enabled = False - self.safe_cmd_vel.publish(Twist.zero()) - logger.info("M20 command output disabled") - def _basic_server_request( self, *, diff --git a/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp index 89276a8a50..45d0831347 100644 --- a/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp +++ b/dimos/robot/deeprobotics/m20/onboard/drdds-zenoh-bridge/cpp/main.cpp @@ -76,7 +76,7 @@ constexpr char kHesStatusTopic[] = "/HES_STATUS"; constexpr char kBodyKey[] = "dimos/slam_body_points/sensor_msgs.PointCloud2"; constexpr char kOdometryKey[] = "dimos/slam_odom/nav_msgs.Odometry"; constexpr char kTfKey[] = "dimos/tf/tf2_msgs.TFMessage"; -constexpr char kSafeCmdVelKey[] = "dimos/safe_cmd_vel/geometry_msgs.Twist"; +constexpr char kCmdVelKey[] = "dimos/cmd_vel/geometry_msgs.Twist"; constexpr char kMotionStateCmdKey[] = "dimos/motion_state_cmd/std_msgs.Int32"; constexpr char kGaitCmdKey[] = "dimos/gait_cmd/std_msgs.UInt32"; constexpr char kCommandReadyKey[] = "dimos/command_ready/std_msgs.Bool"; @@ -652,8 +652,8 @@ int main() { M20ControlBridge control(&command_ready, &motion_state, &gait_state); control.start(); - InputPort safe_cmd_vel( - kSafeCmdVelKey, "safe_cmd_vel", + InputPort cmd_vel( + kCmdVelKey, "cmd_vel", [&control](const uint8_t* data, size_t len) { return control.on_command(data, len); }); @@ -667,7 +667,7 @@ int main() { [&control](const uint8_t* data, size_t len) { return control.on_gait_command(data, len); }); - std::vector input_ports{&safe_cmd_vel, &motion_state_cmd, &gait_cmd}; + std::vector input_ports{&cmd_vel, &motion_state_cmd, &gait_cmd}; for (InputPort* port : input_ports) { if (!declare_zenoh_subscriber(port)) { g_running.store(false); } } From 828bbb756fb9c7fc892ff157b952bd2530a7e351 Mon Sep 17 00:00:00 2001 From: Nabla7 Date: Wed, 2 Sep 2026 13:41:55 +0800 Subject: [PATCH 15/15] fix(robot): start M20 bridge with localization --- dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service b/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service index cc04ce8d45..69f8df6615 100644 --- a/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service +++ b/dimos/robot/deeprobotics/m20/deploy/drdds-zenoh-bridge.service @@ -16,4 +16,4 @@ RestartSec=3 TimeoutStopSec=5 [Install] -WantedBy=multi-user.target +WantedBy=multi-user.target localization.service