From 02a3c8d80288fdb5c04b9629d295c9d456b95f18 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 7 Nov 2026 21:28:51 +0000 Subject: [PATCH 1/2] wip(zenoh): ROS 2 -> Zenoh telemetry gateway example (#421) Draft for #421, not yet run against real ROS 2: - examples/ros2_gateway.rs: geometry_msgs/msg/Twist on /cmd_vel over ros2://, stored as a typed record, republished as JSON on zenoh://telemetry/cmd_vel on the same Zenoh session, and served read-only over AimX on a Unix socket for `aimdb watch`. - examples/zenoh_sub.rs: prints samples on a key expression, a stand-in for z_sub. - tests/interop: the gateway against ros2 topic pub in domain 42, asserting the JSON payload and the aimdb watch output; the image installs geometry_msgs; make ros2-interop builds the aimdb CLI. Still open: the walkthrough, the README link, and an interop run. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01WGkDJ6wdeKyJ6ZH7uXTMDi --- Cargo.lock | 1 + Makefile | 1 + aimdb-zenoh-connector/Cargo.toml | 10 ++ .../examples/ros2_gateway.rs | 100 ++++++++++++++++++ aimdb-zenoh-connector/examples/zenoh_sub.rs | 34 ++++++ .../tests/interop/Dockerfile | 5 +- .../tests/interop/ros2_interop.sh | 50 ++++++++- 7 files changed, 199 insertions(+), 2 deletions(-) create mode 100644 aimdb-zenoh-connector/examples/ros2_gateway.rs create mode 100644 aimdb-zenoh-connector/examples/zenoh_sub.rs diff --git a/Cargo.lock b/Cargo.lock index 618271c0..1420099f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -451,6 +451,7 @@ dependencies = [ "aimdb-core", "aimdb-data-contracts", "aimdb-tokio-adapter", + "aimdb-uds-connector", "defmt 1.1.1", "serde", "tokio", diff --git a/Makefile b/Makefile index 9916f697..5de300a7 100644 --- a/Makefile +++ b/Makefile @@ -866,6 +866,7 @@ DISTRO ?= lyrical ros2-interop: @printf "$(GREEN)ROS 2 interop on $(DISTRO)...$(NC)\n" cargo build --package aimdb-zenoh-connector --features "std" --examples + cargo build --package aimdb-cli ./aimdb-zenoh-connector/tests/interop/ros2_interop.sh $(DISTRO) ## Drift guards diff --git a/aimdb-zenoh-connector/Cargo.toml b/aimdb-zenoh-connector/Cargo.toml index 6dda880a..ebcd9f72 100644 --- a/aimdb-zenoh-connector/Cargo.toml +++ b/aimdb-zenoh-connector/Cargo.toml @@ -63,6 +63,8 @@ tokio = { workspace = true, features = ["macros", "rt-multi-thread"] } # `tests/ros2_outbound.rs`: derived ROS types and a non-CDR codec. aimdb-data-contracts = { path = "../aimdb-data-contracts", features = ["ros2", "linkable-json"] } serde = { version = "1", features = ["derive"] } +# `examples/ros2_gateway.rs`: AimX over a Unix socket, for `aimdb watch`. +aimdb-uds-connector = { path = "../aimdb-uds-connector" } # The `register` bound, as a compile error. trybuild = "1.0" @@ -73,3 +75,11 @@ required-features = ["std"] [[example]] name = "ros2_listener" required-features = ["std"] + +[[example]] +name = "ros2_gateway" +required-features = ["std"] + +[[example]] +name = "zenoh_sub" +required-features = ["std"] diff --git a/aimdb-zenoh-connector/examples/ros2_gateway.rs b/aimdb-zenoh-connector/examples/ros2_gateway.rs new file mode 100644 index 00000000..21a4f034 --- /dev/null +++ b/aimdb-zenoh-connector/examples/ros2_gateway.rs @@ -0,0 +1,100 @@ +//! A ROS 2 → Zenoh telemetry gateway: every `geometry_msgs/msg/Twist` on +//! `/cmd_vel` becomes a typed AimDB record, is republished as JSON on the +//! plain Zenoh key `telemetry/cmd_vel`, and can be watched over AimX. +//! +//! `ros2://` and `zenoh://` share one session to the router at +//! `ZENOH_ENDPOINT` (default `tcp/127.0.0.1:7447`). The ROS domain comes from +//! `ROS_DOMAIN_ID` (default 0). AimX listens on `AIMX_SOCKET` (default +//! `/tmp/aimdb-ros2-gateway.sock`), read-only. +//! +//! ```text +//! cargo run -p aimdb-zenoh-connector --features std --example ros2_gateway +//! ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \ +//! "{linear: {x: 0.5}, angular: {z: 0.3}}" +//! aimdb watch robot.cmd_vel +//! cargo run -p aimdb-zenoh-connector --features std --example zenoh_sub -- 'telemetry/**' +//! ``` + +use std::sync::Arc; + +use aimdb_core::buffer::BufferCfg; +use aimdb_core::remote::{AimxConfig, SecurityPolicy}; +use aimdb_core::AimDbBuilder; +use aimdb_data_contracts::link_codecs::Json; +use aimdb_data_contracts::{LinkCodecRegistrarExt, LinkableRegistrarExt, RosMessage}; +use aimdb_tokio_adapter::{TokioAdapter, TokioRecordRegistrarExt}; +use aimdb_uds_connector::UdsServer; +use aimdb_zenoh_connector::{Ros2Node, ZenohConnector}; +use serde::{Deserialize, Serialize}; + +/// `geometry_msgs/msg/Vector3`: `float64 x, y, z`. +#[derive(Clone, Debug, Serialize, Deserialize, RosMessage)] +#[ros( + type = "geometry_msgs/msg/Vector3", + hash = "RIHS01_cc12fe83e4c02719f1ce8070bfd14aecd40f75a96696a67a2a1f37f7dbb0765d" +)] +struct Vector3 { + x: f64, + y: f64, + z: f64, +} + +/// `geometry_msgs/msg/Twist`, with the hash Jazzy and Lyrical publish +/// (`ros2 topic info -v /cmd_vel`). The fields are in `.msg` order: CDR is +/// positional, and `Vector3` is encoded inline. +#[derive(Clone, Debug, Serialize, Deserialize, RosMessage)] +#[ros( + type = "geometry_msgs/msg/Twist", + hash = "RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a" +)] +struct Twist { + linear: Vector3, + angular: Vector3, +} + +#[tokio::main] +async fn main() { + let endpoint = std::env::var("ZENOH_ENDPOINT").unwrap_or_else(|_| "tcp/127.0.0.1:7447".into()); + let socket = + std::env::var("AIMX_SOCKET").unwrap_or_else(|_| "/tmp/aimdb-ros2-gateway.sock".into()); + + // One Zenoh session: the ROS view is derived from the plain connector. + let zenoh = ZenohConnector::new(endpoint.as_str()); + let ros2 = zenoh + .ros2(Ros2Node::new("telemetry_gateway").namespace("/aimdb")) + .register::(); + + // AimX over a Unix socket, for `aimdb watch`. Read-only: ROS is the only + // writer of the record. + let _ = std::fs::remove_file(&socket); + let aimx = UdsServer::from_config( + AimxConfig::uds_default() + .socket_path(socket.as_str()) + .security_policy(SecurityPolicy::read_only()), + ); + + let mut builder = AimDbBuilder::new() + .runtime(Arc::new(TokioAdapter::new().unwrap())) + .with_connector(zenoh) + .with_connector(ros2) + .with_connector(aimx); + builder.configure::("robot.cmd_vel", |reg| { + reg.buffer(BufferCfg::SpmcRing { capacity: 16 }) + .with_remote_access() + .linked_from("ros2://cmd_vel") // CDR, as a ROS subscription + .linked_to_with("zenoh://telemetry/cmd_vel", Json); // JSON, plain Zenoh + }); + let (db, runner) = builder.build().await.expect("build"); + let mut reader = db.subscribe::("robot.cmd_vel").expect("reader"); + tokio::spawn(runner.run()); + + println!("ros2_gateway: /cmd_vel -> robot.cmd_vel -> telemetry/cmd_vel"); + println!("ros2_gateway: router {endpoint}, AimX unix://{socket}"); + while let Ok(twist) = reader.recv().await { + let Twist { linear, angular } = &twist.value; + println!( + "cmd_vel: linear ({}, {}, {}) angular ({}, {}, {})", + linear.x, linear.y, linear.z, angular.x, angular.y, angular.z + ); + } +} diff --git a/aimdb-zenoh-connector/examples/zenoh_sub.rs b/aimdb-zenoh-connector/examples/zenoh_sub.rs new file mode 100644 index 00000000..3a807a2c --- /dev/null +++ b/aimdb-zenoh-connector/examples/zenoh_sub.rs @@ -0,0 +1,34 @@ +//! Prints every sample on a Zenoh key expression as text, through the router +//! at `ZENOH_ENDPOINT` (default `tcp/127.0.0.1:7447`); a stand-in for +//! `z_sub` when the Zenoh tools are not installed. +//! +//! ```text +//! cargo run -p aimdb-zenoh-connector --features std --example zenoh_sub -- 'telemetry/**' +//! ``` + +#[tokio::main] +async fn main() { + let endpoint = std::env::var("ZENOH_ENDPOINT").unwrap_or_else(|_| "tcp/127.0.0.1:7447".into()); + let key = std::env::args().nth(1).unwrap_or_else(|| "**".into()); + + let mut config = zenoh::Config::default(); + config.insert_json5("mode", r#""client""#).unwrap(); + config + .insert_json5("connect/endpoints", &format!(r#"["{endpoint}"]"#)) + .unwrap(); + config + .insert_json5("scouting/multicast/enabled", "false") + .unwrap(); + + let session = zenoh::open(config).await.expect("open"); + let subscriber = session.declare_subscriber(&key).await.expect("subscribe"); + println!("zenoh_sub: {key} on {endpoint}"); + while let Ok(sample) = subscriber.recv_async().await { + let payload = sample.payload().to_bytes(); + println!( + "{}: {}", + sample.key_expr(), + String::from_utf8_lossy(&payload) + ); + } +} diff --git a/aimdb-zenoh-connector/tests/interop/Dockerfile b/aimdb-zenoh-connector/tests/interop/Dockerfile index 6c1081a7..0b0f1439 100644 --- a/aimdb-zenoh-connector/tests/interop/Dockerfile +++ b/aimdb-zenoh-connector/tests/interop/Dockerfile @@ -1,8 +1,11 @@ # A ROS 2 distribution with rmw_zenoh, for tests/interop/ros2_interop.sh. +# geometry_msgs is for the ros2_gateway example's Twist on /cmd_vel. ARG DISTRO=lyrical FROM ros:${DISTRO} ARG DISTRO RUN apt-get update \ - && apt-get install -y --no-install-recommends ros-${DISTRO}-rmw-zenoh-cpp \ + && apt-get install -y --no-install-recommends \ + ros-${DISTRO}-rmw-zenoh-cpp \ + ros-${DISTRO}-geometry-msgs \ && rm -rf /var/lib/apt/lists/* ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp diff --git a/aimdb-zenoh-connector/tests/interop/ros2_interop.sh b/aimdb-zenoh-connector/tests/interop/ros2_interop.sh index 281636e5..76de2671 100755 --- a/aimdb-zenoh-connector/tests/interop/ros2_interop.sh +++ b/aimdb-zenoh-connector/tests/interop/ros2_interop.sh @@ -14,6 +14,7 @@ set -euo pipefail DISTRO=${1:?usage: ros2_interop.sh } HERE=$(cd "$(dirname "$0")" && pwd) EXAMPLES=${EXAMPLES:-target/debug/examples} +AIMDB=${AIMDB:-target/debug/aimdb} IMAGE=${IMAGE:-aimdb-ros2-interop:$DISTRO} PREFIX=aimdb-interop-$DISTRO-$$ PIDS=() @@ -36,7 +37,8 @@ fail() { # and read back with `docker logs`, which works on every Docker setup. ros() { local name=$PREFIX-cli-$RANDOM - docker run -d --name "$name" --network host "$IMAGE" "$@" >/dev/null + docker run -d --name "$name" --network host \ + -e ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-0}" "$IMAGE" "$@" >/dev/null docker wait "$name" >/dev/null docker logs "$name" 2>&1 docker container rm "$name" >/dev/null @@ -115,4 +117,50 @@ rm -f "$listener_log" [[ $received -eq 3 ]] || fail "the listener received $received of 3 messages" echo "ok: subscription (topic info -v, topic pub, 3 of 3 received)" +# --- The gateway (examples/ros2_gateway.md): ROS publishes a Twist on +# /cmd_vel in domain 42; AimDB stores it, republishes it as JSON on plain +# Zenoh and serves it over AimX. +export ROS_DOMAIN_ID=42 +socket=$(mktemp -u /tmp/aimdb-interop-XXXXXX.sock) +gateway_log=$(mktemp) +zenoh_log=$(mktemp) +watch_log=$(mktemp) +AIMX_SOCKET=$socket "$EXAMPLES/ros2_gateway" >"$gateway_log" 2>&1 & +PIDS+=($!) +"$EXAMPLES/zenoh_sub" 'telemetry/**' >"$zenoh_log" 2>&1 & +PIDS+=($!) +for _ in $(seq 1 30); do + [[ -S $socket ]] && grep -q '^zenoh_sub:' "$zenoh_log" && break + sleep 1 +done +[[ -S $socket ]] || fail "the gateway did not open $socket" +grep -q '^zenoh_sub:' "$zenoh_log" || fail "zenoh_sub did not subscribe: +$(cat "$zenoh_log")" +"$AIMDB" --connect "unix://$socket" watch robot.cmd_vel --format json --new \ + --count 3 --timeout 60 >"$watch_log" 2>&1 & +watch_pid=$! +PIDS+=($watch_pid) + +ros_until 'Subscription count: 1' ros2 topic info -v /cmd_vel >/dev/null \ + || fail "ros2 topic info -v does not show the gateway's subscription" +ros timeout 30 ros2 topic pub --times 3 -r 2 /cmd_vel geometry_msgs/msg/Twist \ + "{linear: {x: 0.5}, angular: {z: 0.3}}" >/dev/null + +json='{"linear":{"x":0.5,"y":0.0,"z":0.0},"angular":{"x":0.0,"y":0.0,"z":0.3}}' +for _ in $(seq 1 10); do + [[ $(grep -cxF "telemetry/cmd_vel: $json" "$zenoh_log") -ge 3 ]] && break + sleep 1 +done +republished=$(grep -cxF "telemetry/cmd_vel: $json" "$zenoh_log" || true) +[[ $republished -eq 3 ]] || fail "zenoh_sub received $republished of 3 JSON payloads: +$(cat "$zenoh_log")" +wait "$watch_pid" || fail "aimdb watch did not see 3 updates: +$(cat "$watch_log")" +watched=$(grep -c '"value":{"angular":{"x":0.0,"y":0.0,"z":0.3},"linear":{"x":0.5,"y":0.0,"z":0.0}}' "$watch_log" || true) +[[ $watched -eq 3 ]] || fail "aimdb watch printed $watched of 3 Twists: +$(cat "$watch_log")" +rm -f "$gateway_log" "$zenoh_log" "$watch_log" +unset ROS_DOMAIN_ID +echo "ok: gateway (ros2 topic pub, 3 of 3 republished as JSON, aimdb watch)" + echo "PASS ($DISTRO)" From 98f6ac0096aa7db4545c9cafd86e765759d7edfe Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 7 Nov 2026 21:51:27 +0000 Subject: [PATCH 2/2] feat(zenoh): ROS 2 gateway and turtlesim-over-MCP examples (#421) - ros2_gateway: geometry_msgs/Twist on /cmd_vel over ros2://, kept as a SingleLatest record, republished as JSON on zenoh://telemetry/cmd_vel on the same session, and served read-only over AimX. - turtlesim_mcp: turtle.goal (Stop | GoTo { x, y }) is the only record writable over AimX. A transform_join controller turns goal and pose into turtle.cmd_vel, so remote writes to cmd_vel are refused. The pose type follows ROS_DISTRO: turtlesim/msg/Pose on Jazzy, turtlesim_msgs/msg/Pose after it. - zenoh_sub: prints a key expression's samples, a stand-in for z_sub. - docs/ros2-walkthrough.md, linked from the connector README and the ROS 2 guide. - ros2_interop.sh checks the gateway's JSON, aimdb watch and the late snapshot, a GoTo reaching its target and a refused cmd_vel write; the image adds geometry_msgs and turtlesim; make ros2-interop builds the CLI. Passes make ros2-interop on Jazzy and Lyrical. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01WGkDJ6wdeKyJ6ZH7uXTMDi --- aimdb-zenoh-connector/CHANGELOG.md | 11 + aimdb-zenoh-connector/Cargo.toml | 4 + aimdb-zenoh-connector/README.md | 13 +- .../examples/ros2_gateway.rs | 6 +- .../examples/turtlesim_mcp.rs | 262 ++++++++++++++++ .../tests/interop/Dockerfile | 4 +- .../tests/interop/ros2_interop.sh | 48 ++- docs/aimdb-and-ros2.md | 4 + docs/ros2-walkthrough.md | 282 ++++++++++++++++++ 9 files changed, 629 insertions(+), 5 deletions(-) create mode 100644 aimdb-zenoh-connector/examples/turtlesim_mcp.rs create mode 100644 docs/ros2-walkthrough.md diff --git a/aimdb-zenoh-connector/CHANGELOG.md b/aimdb-zenoh-connector/CHANGELOG.md index e9607a68..03c6b51c 100644 --- a/aimdb-zenoh-connector/CHANGELOG.md +++ b/aimdb-zenoh-connector/CHANGELOG.md @@ -7,6 +7,17 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added + +- **ROS 2 examples and walkthrough (#421).** `ros2_gateway` keeps + `geometry_msgs/Twist` from `/cmd_vel` as a record and republishes it as + JSON on plain Zenoh, on one session, with AimX for `aimdb watch`. + `turtlesim_mcp` drives turtlesim with `GoTo`/`Stop` goals written over AimX, + for example by Claude through `aimdb-mcp`; a `transform_join` controller + turns goal and pose into `cmd_vel`. `zenoh_sub` prints a key expression's + samples. `docs/ros2-walkthrough.md` runs both, and `make ros2-interop` + checks them against Jazzy and Lyrical. + ## [0.1.0] - 2026-11-03 ### Added diff --git a/aimdb-zenoh-connector/Cargo.toml b/aimdb-zenoh-connector/Cargo.toml index ebcd9f72..4cfc643e 100644 --- a/aimdb-zenoh-connector/Cargo.toml +++ b/aimdb-zenoh-connector/Cargo.toml @@ -83,3 +83,7 @@ required-features = ["std"] [[example]] name = "zenoh_sub" required-features = ["std"] + +[[example]] +name = "turtlesim_mcp" +required-features = ["std"] diff --git a/aimdb-zenoh-connector/README.md b/aimdb-zenoh-connector/README.md index 8eed63f8..002448d0 100644 --- a/aimdb-zenoh-connector/README.md +++ b/aimdb-zenoh-connector/README.md @@ -156,7 +156,16 @@ cargo test -p aimdb-zenoh-connector --features std # includes an in-process ro make ros2-interop DISTRO=lyrical # against real ROS 2, in Docker ``` -The examples `ros2_talker` and `ros2_listener` are a publisher and a -subscription for trying the connector against a ROS 2 host. +## Examples + +| Example | What it shows | +|---|---| +| `ros2_talker`, `ros2_listener` | A ROS publisher and a ROS subscription | +| [`ros2_gateway`](examples/ros2_gateway.rs) | `geometry_msgs/Twist` from `/cmd_vel` kept as a record, republished as JSON on plain Zenoh on the same session, watched with `aimdb watch` | +| [`turtlesim_mcp`](examples/turtlesim_mcp.rs) | turtlesim driven by goals written over AimX, for example by Claude through `aimdb-mcp`; an AimDB controller turns goal and pose into `cmd_vel` | +| [`zenoh_sub`](examples/zenoh_sub.rs) | Prints the samples on a key expression, a stand-in for `z_sub` | + +[The ROS 2 walkthrough](https://github.com/aimdb-dev/aimdb/blob/main/docs/ros2-walkthrough.md) +runs the gateway and the turtle step by step; `make ros2-interop` checks it. Design: [053 — Zenoh connector](https://github.com/aimdb-dev/aimdb/blob/main/docs/design/053-zenoh-connector.md). diff --git a/aimdb-zenoh-connector/examples/ros2_gateway.rs b/aimdb-zenoh-connector/examples/ros2_gateway.rs index 21a4f034..51db28c2 100644 --- a/aimdb-zenoh-connector/examples/ros2_gateway.rs +++ b/aimdb-zenoh-connector/examples/ros2_gateway.rs @@ -14,6 +14,8 @@ //! aimdb watch robot.cmd_vel //! cargo run -p aimdb-zenoh-connector --features std --example zenoh_sub -- 'telemetry/**' //! ``` +//! +//! Walkthrough: `docs/ros2-walkthrough.md`. use std::sync::Arc; @@ -78,8 +80,10 @@ async fn main() { .with_connector(zenoh) .with_connector(ros2) .with_connector(aimx); + // SingleLatest: the record is the robot's current command, which + // `aimdb record get` reads and a late `aimdb watch` receives first. builder.configure::("robot.cmd_vel", |reg| { - reg.buffer(BufferCfg::SpmcRing { capacity: 16 }) + reg.buffer(BufferCfg::SingleLatest) .with_remote_access() .linked_from("ros2://cmd_vel") // CDR, as a ROS subscription .linked_to_with("zenoh://telemetry/cmd_vel", Json); // JSON, plain Zenoh diff --git a/aimdb-zenoh-connector/examples/turtlesim_mcp.rs b/aimdb-zenoh-connector/examples/turtlesim_mcp.rs new file mode 100644 index 00000000..7ae1d6db --- /dev/null +++ b/aimdb-zenoh-connector/examples/turtlesim_mcp.rs @@ -0,0 +1,262 @@ +//! Drives turtlesim's turtle with goals set over AimX, for example by Claude +//! through `aimdb-mcp`. The caller says where to go; this process decides how +//! to move. +//! +//! ```text +//! turtle.goal "Stop" | {"GoTo": {"x": 8.0, "y": 8.0}} writable over AimX +//! /turtle1/pose ──▶ turtle.pose read-only +//! turtle.goal + turtle.pose ──controller──▶ turtle.cmd_vel ──▶ /turtle1/cmd_vel +//! ``` +//! +//! `turtle.cmd_vel` is the output of a transform, so AimDB refuses remote +//! writes to it: the only way to move the turtle is a goal, and the speed +//! limits live in the controller. +//! +//! The router is `ZENOH_ENDPOINT` (default `tcp/127.0.0.1:7447`), the domain +//! `ROS_DOMAIN_ID` (default 0), and AimX listens on `AIMX_SOCKET` (default +//! `/tmp/aimdb-turtlesim.sock`). The pose is `turtlesim/msg/Pose` on Jazzy +//! and `turtlesim_msgs/msg/Pose` on later distros, chosen by `ROS_DISTRO` +//! (default Jazzy). +//! +//! ```text +//! ros2 run turtlesim turtlesim_node +//! cargo run -p aimdb-zenoh-connector --features std --example turtlesim_mcp +//! aimdb record set turtle.goal '{"GoTo": {"x": 8.0, "y": 8.0}}' +//! aimdb watch turtle.cmd_vel +//! ``` +//! +//! Walkthrough, including Claude through `aimdb-mcp`: `docs/ros2-walkthrough.md`. + +use std::f32::consts::{FRAC_PI_4, PI}; +use std::fmt::Debug; +use std::sync::Arc; + +use aimdb_core::buffer::BufferCfg; +use aimdb_core::remote::{AimxConfig, SecurityPolicy}; +use aimdb_core::AimDbBuilder; +use aimdb_data_contracts::{LinkableRegistrarExt, RosMessage}; +use aimdb_tokio_adapter::{TokioAdapter, TokioRecordRegistrarExt}; +use aimdb_uds_connector::UdsServer; +use aimdb_zenoh_connector::{Ros2Connector, Ros2Node}; +use serde::de::DeserializeOwned; +use serde::{Deserialize, Serialize}; + +/// What the controller needs from a pose, whichever package it comes from. +trait TurtlePose: + RosMessage + Clone + Debug + Serialize + DeserializeOwned + Send + Sync + 'static +{ + fn position(&self) -> (f32, f32, f32); +} + +/// turtlesim's pose, as Jazzy publishes it. +#[derive(Clone, Debug, Serialize, Deserialize, RosMessage)] +#[ros( + type = "turtlesim/msg/Pose", + hash = "RIHS01_739beba26bcba6920404ba722b7b8321348512f92ea5be235c47251940dd8aa9" +)] +struct JazzyPose { + x: f32, + y: f32, + theta: f32, + linear_velocity: f32, + angular_velocity: f32, +} + +/// The same message after it moved to `turtlesim_msgs` (Lyrical): a new type +/// name, so a new hash. +#[derive(Clone, Debug, Serialize, Deserialize, RosMessage)] +#[ros( + type = "turtlesim_msgs/msg/Pose", + hash = "RIHS01_37b07ace09a0ed2c5b774b1410dec972dfd1d292de5656ebc2b075ac646e09c8" +)] +struct Pose { + x: f32, + y: f32, + theta: f32, + linear_velocity: f32, + angular_velocity: f32, +} + +impl TurtlePose for JazzyPose { + fn position(&self) -> (f32, f32, f32) { + (self.x, self.y, self.theta) + } +} + +impl TurtlePose for Pose { + fn position(&self) -> (f32, f32, f32) { + (self.x, self.y, self.theta) + } +} + +/// `geometry_msgs/msg/Vector3`. +#[derive(Clone, Debug, Default, Serialize, Deserialize, RosMessage)] +#[ros( + type = "geometry_msgs/msg/Vector3", + hash = "RIHS01_cc12fe83e4c02719f1ce8070bfd14aecd40f75a96696a67a2a1f37f7dbb0765d" +)] +struct Vector3 { + x: f64, + y: f64, + z: f64, +} + +/// `geometry_msgs/msg/Twist`, with the hash Jazzy and Lyrical publish. +#[derive(Clone, Debug, Default, Serialize, Deserialize, RosMessage)] +#[ros( + type = "geometry_msgs/msg/Twist", + hash = "RIHS01_9c45bf16fe0983d80e3cfe750d6835843d265a9a6c46bd2e609fcddde6fb8d2a" +)] +struct Twist { + linear: Vector3, + angular: Vector3, +} + +/// What the caller wants. Not a ROS type: it never leaves AimDB. +#[derive(Clone, Debug, Serialize, Deserialize)] +enum Goal { + Stop, + GoTo { x: f32, y: f32 }, +} + +/// Fastest forward speed, in turtlesim units per second. +const MAX_LINEAR: f32 = 2.0; +/// Fastest turn, in radians per second. +const MAX_ANGULAR: f32 = 2.0; +/// Close enough to the target. +const TOLERANCE: f32 = 0.05; +/// Targets stay inside turtlesim's 11 × 11 arena, away from the walls. +const ARENA: (f32, f32) = (0.5, 10.5); + +/// One controller step: the command that moves `pose` toward `goal`, or +/// `None` once the turtle is there or told to stop. +fn step(goal: &Goal, (px, py, theta): (f32, f32, f32)) -> Option { + let Goal::GoTo { x, y } = *goal else { + return None; + }; + if !(x.is_finite() && y.is_finite()) { + return None; + } + let (dx, dy) = ( + x.clamp(ARENA.0, ARENA.1) - px, + y.clamp(ARENA.0, ARENA.1) - py, + ); + let distance = dx.hypot(dy); + if distance < TOLERANCE { + return None; + } + // Heading error, wrapped to (-π, π]. + let mut error = dy.atan2(dx) - theta; + while error > PI { + error -= 2.0 * PI; + } + while error <= -PI { + error += 2.0 * PI; + } + // Turn toward the target; drive only when roughly facing it. + let angular = (4.0 * error).clamp(-MAX_ANGULAR, MAX_ANGULAR); + let linear = if error.abs() < FRAC_PI_4 { + (1.5 * distance).min(MAX_LINEAR) * error.cos() + } else { + 0.0 + }; + Some(Twist { + linear: Vector3 { + x: linear.into(), + ..Vector3::default() + }, + angular: Vector3 { + z: angular.into(), + ..Vector3::default() + }, + }) +} + +#[tokio::main] +async fn main() { + // turtlesim's messages moved to `turtlesim_msgs` after Jazzy. A wrong type + // fails silently, so the choice is printed. + match std::env::var("ROS_DISTRO").as_deref() { + Ok("jazzy") | Err(_) => run::().await, + Ok(_) => run::().await, + } +} + +async fn run() { + let endpoint = std::env::var("ZENOH_ENDPOINT").unwrap_or_else(|_| "tcp/127.0.0.1:7447".into()); + let socket = + std::env::var("AIMX_SOCKET").unwrap_or_else(|_| "/tmp/aimdb-turtlesim.sock".into()); + + let ros2 = Ros2Connector::new( + endpoint.as_str(), + Ros2Node::new("turtle_pilot").namespace("/aimdb"), + ) + .register::

() + .register::(); + + // AimX: every record is readable, only the goal is writable. + let _ = std::fs::remove_file(&socket); + let aimx = UdsServer::from_config( + AimxConfig::uds_default() + .socket_path(socket.as_str()) + .security_policy(SecurityPolicy::read_write().with_writable_key("turtle.goal")), + ); + + let mut builder = AimDbBuilder::new() + .runtime(Arc::new(TokioAdapter::new().unwrap())) + .with_connector(ros2) + .with_connector(aimx); + builder.configure::("turtle.goal", |reg| { + reg.buffer(BufferCfg::SingleLatest).with_remote_access(); + }); + builder.configure::

("turtle.pose", |reg| { + reg.buffer(BufferCfg::SingleLatest) + .with_remote_access() + .linked_from("ros2://turtle1/pose"); + }); + // Only the newest velocity command matters. + builder.configure::("turtle.cmd_vel", |reg| { + reg.buffer(BufferCfg::SingleLatest) + .with_remote_access() + .transform_join(|join| { + join.input::("turtle.goal") + .input::

("turtle.pose") + .on_triggers(|mut rx, producer| async move { + let mut goal = Goal::Stop; + let mut pose = None; + let mut moving = false; + while let Ok(trigger) = rx.recv().await { + if let Some(g) = trigger.as_input::() { + println!("goal: {g:?}"); + goal = g.clone(); + } else if let Some(p) = trigger.as_input::

() { + pose = Some(p.position()); + } + let Some(pose) = pose else { continue }; + match step(&goal, pose) { + Some(twist) => { + moving = true; + producer.produce(twist); + } + // One zero command on arrival or Stop, then + // quiet until the next goal. + None if moving => { + moving = false; + println!("stopped at ({:.2}, {:.2})", pose.0, pose.1); + producer.produce(Twist::default()); + } + None => {} + } + } + }) + }) + .linked_to("ros2://turtle1/cmd_vel"); + }); + let (_db, runner) = builder.build().await.expect("build"); + println!( + "turtlesim_mcp: /turtle1/pose ({}) -> controller -> /turtle1/cmd_vel", + P::ROS_TYPE_NAME + ); + println!("turtlesim_mcp: router {endpoint}, AimX unix://{socket} (turtle.goal writable)"); + runner.run().await; +} diff --git a/aimdb-zenoh-connector/tests/interop/Dockerfile b/aimdb-zenoh-connector/tests/interop/Dockerfile index 0b0f1439..f06e5586 100644 --- a/aimdb-zenoh-connector/tests/interop/Dockerfile +++ b/aimdb-zenoh-connector/tests/interop/Dockerfile @@ -1,5 +1,6 @@ # A ROS 2 distribution with rmw_zenoh, for tests/interop/ros2_interop.sh. -# geometry_msgs is for the ros2_gateway example's Twist on /cmd_vel. +# geometry_msgs and turtlesim are for the ros2_gateway and turtlesim_mcp +# examples. ARG DISTRO=lyrical FROM ros:${DISTRO} ARG DISTRO @@ -7,5 +8,6 @@ RUN apt-get update \ && apt-get install -y --no-install-recommends \ ros-${DISTRO}-rmw-zenoh-cpp \ ros-${DISTRO}-geometry-msgs \ + ros-${DISTRO}-turtlesim \ && rm -rf /var/lib/apt/lists/* ENV RMW_IMPLEMENTATION=rmw_zenoh_cpp diff --git a/aimdb-zenoh-connector/tests/interop/ros2_interop.sh b/aimdb-zenoh-connector/tests/interop/ros2_interop.sh index 76de2671..26bad10d 100755 --- a/aimdb-zenoh-connector/tests/interop/ros2_interop.sh +++ b/aimdb-zenoh-connector/tests/interop/ros2_interop.sh @@ -159,8 +159,54 @@ $(cat "$watch_log")" watched=$(grep -c '"value":{"angular":{"x":0.0,"y":0.0,"z":0.3},"linear":{"x":0.5,"y":0.0,"z":0.0}}' "$watch_log" || true) [[ $watched -eq 3 ]] || fail "aimdb watch printed $watched of 3 Twists: $(cat "$watch_log")" +# A watch that starts after the last message gets it as a snapshot. +late=$("$AIMDB" --connect "unix://$socket" watch robot.cmd_vel --format json \ + --count 1 --timeout 10 2>/dev/null) || fail "a late aimdb watch got no snapshot" +grep -q '"linear":{"x":0.5' <<<"$late" || fail "the late snapshot is not the last Twist: +$late" rm -f "$gateway_log" "$zenoh_log" "$watch_log" unset ROS_DOMAIN_ID -echo "ok: gateway (ros2 topic pub, 3 of 3 republished as JSON, aimdb watch)" +echo "ok: gateway (ros2 topic pub, 3 of 3 republished as JSON, aimdb watch, late snapshot)" + +# --- turtlesim_mcp (docs/ros2-walkthrough.md): a goal written over AimX +# drives the turtle there; the controller's output refuses remote writes. +docker run -d --name "$PREFIX-turtlesim" --network host \ + -e QT_QPA_PLATFORM=offscreen "$IMAGE" ros2 run turtlesim turtlesim_node >/dev/null +socket=$(mktemp -u /tmp/aimdb-interop-XXXXXX.sock) +pilot_log=$(mktemp) +ROS_DISTRO=$DISTRO AIMX_SOCKET=$socket "$EXAMPLES/turtlesim_mcp" >"$pilot_log" 2>&1 & +PIDS+=($!) +aimdb() { "$AIMDB" --connect "unix://$socket" "$@"; } + +# Prints the turtle's "x y", once turtle.pose holds a value. +pose() { + aimdb record get turtle.pose 2>/dev/null \ + | python3 -c 'import json,sys; v=json.load(sys.stdin)["value"]; print(v["x"], v["y"])' 2>/dev/null +} +for _ in $(seq 1 30); do pose >/dev/null && break; sleep 1; done +start=$(pose) || fail "turtle.pose never received /turtle1/pose: +$(cat "$pilot_log")" + +aimdb record set turtle.goal '{"GoTo": {"x": 8.0, "y": 8.0}}' >/dev/null \ + || fail "turtle.goal is not writable over AimX" +arrived= +for _ in $(seq 1 30); do + if pose | awk '{ exit !((($1-8)^2 + ($2-8)^2) < 0.1^2) }'; then + arrived=$(pose) + break + fi + sleep 1 +done +[[ -n $arrived ]] || fail "the turtle did not reach (8, 8) from ($start), at ($(pose)): +$(cat "$pilot_log")" + +if out=$(aimdb record set turtle.cmd_vel \ + '{"linear":{"x":9.0,"y":0.0,"z":0.0},"angular":{"x":0.0,"y":0.0,"z":0.0}}' 2>&1); then + fail "turtle.cmd_vel accepted a remote write" +fi +grep -q 'denied' <<<"$out" || fail "the write to turtle.cmd_vel failed for another reason: +$out" +rm -f "$pilot_log" +echo "ok: turtlesim_mcp (GoTo from ($start) to ($arrived), cmd_vel write denied)" echo "PASS ($DISTRO)" diff --git a/docs/aimdb-and-ros2.md b/docs/aimdb-and-ros2.md index 8f08f891..5ba346fb 100644 --- a/docs/aimdb-and-ros2.md +++ b/docs/aimdb-and-ros2.md @@ -127,6 +127,10 @@ $ ros2 topic pub --once /cell4/spindle_cmd cell_msgs/msg/SpindleCommand "{rpm: 1 distribution in Docker, with the connector's `ros2_talker` and `ros2_listener` examples. +[The ROS 2 walkthrough](ros2-walkthrough.md) runs two complete examples: a +gateway that republishes `/cmd_vel` as JSON, and turtlesim driven by goals +written over AimX, for example by Claude through `aimdb-mcp`. + ## Not yet Services, actions, `TRANSIENT_LOCAL` (latched topics), Humble, and a diff --git a/docs/ros2-walkthrough.md b/docs/ros2-walkthrough.md new file mode 100644 index 00000000..0c9519fe --- /dev/null +++ b/docs/ros2-walkthrough.md @@ -0,0 +1,282 @@ +# Walkthrough: observe and command a ROS 2 robot from AimDB + +Two examples in `aimdb-zenoh-connector/examples/` run against a real ROS 2 +system: + +- **`ros2_gateway`** observes. It subscribes to `geometry_msgs/msg/Twist` on + `/cmd_vel`, keeps each message as a typed record, and republishes it as JSON + on a plain Zenoh key for consumers that don't speak ROS. +- **`turtlesim_mcp`** commands. A caller, for example Claude through + `aimdb-mcp`, writes a *goal*. A controller in AimDB turns goal and pose into + velocity commands for turtlesim. The caller decides where to go, AimDB + decides how to move, and ROS 2 moves the turtle. + +Neither example needs ROS installed on the AimDB side, only a Zenoh router +both sides reach. `make ros2-interop` runs every command on this page against +Jazzy and Lyrical in CI, so the outputs below are the ones the test checks. + +## Prerequisites + +- ROS 2 **Jazzy** or **Lyrical** with `rmw_zenoh_cpp`, `geometry_msgs` and + `turtlesim`: + + ```text + sudo apt install ros-jazzy-rmw-zenoh-cpp ros-jazzy-turtlesim + ``` + + Humble has no type hashes and is not supported. +- A Rust toolchain and this repository. Build the examples and install the + CLI and the MCP server: + + ```text + cargo build -p aimdb-zenoh-connector --features std --examples + cargo install --path tools/aimdb-cli # the `aimdb` command + cargo install --path tools/aimdb-mcp # the `aimdb-mcp` command + ``` + +The commands below run the examples from `target/debug/examples/`; +`cargo run -p aimdb-zenoh-connector --features std --example ` works too. + +## 1. Start the router + +Every ROS node and every AimDB process connects to one Zenoh router. In each +ROS terminal: + +```text +source /opt/ros/jazzy/setup.bash +export RMW_IMPLEMENTATION=rmw_zenoh_cpp +``` + +Then start the router and leave it running: + +```text +ros2 run rmw_zenoh_cpp rmw_zenohd +``` + +It listens on `tcp/[::]:7447`. On a host without IPv6, point +`ZENOH_ROUTER_CONFIG_URI` at a copy of +`$(ros2 pkg prefix rmw_zenoh_cpp)/share/rmw_zenoh_cpp/config/DEFAULT_RMW_ZENOH_ROUTER_CONFIG.json5` +that listens on `tcp/0.0.0.0:7447`. + +## 2. Configuration + +Both examples read two variables: + +| Variable | Default | Meaning | +|---|---|---| +| `ZENOH_ENDPOINT` | `tcp/127.0.0.1:7447` | The router. Set it when AimDB runs on another machine, e.g. `tcp/192.168.1.20:7447` | +| `ROS_DOMAIN_ID` | `0` | The ROS domain, read the way `rcl` reads it. It must match the ROS side | + +**A wrong domain or type hash fails silently**: the two sides never meet, and +nothing reports an error. If nothing arrives, check `ROS_DOMAIN_ID` on both +sides first, then compare the hash in the example with +`ros2 topic info -v `. + +`AIMX_SOCKET` sets the Unix socket the `aimdb` CLI and `aimdb-mcp` connect +to. Its default is `/tmp/aimdb-ros2-gateway.sock` for the gateway and +`/tmp/aimdb-turtlesim.sock` for the turtle. The CLI finds sockets in `/tmp` +on its own; pass `--connect unix://` when more than one is running. + +## 3. Observe: the gateway + +```text +/cmd_vel ──ros2://──▶ robot.cmd_vel ──zenoh:// (JSON)──▶ telemetry/cmd_vel + │ + └──AimX──▶ aimdb watch +``` + +The whole configuration is one record with two links. Both links share one +Zenoh session, because the ROS view is derived from the plain connector: + +```rust +let zenoh = ZenohConnector::new(endpoint.as_str()); +let ros2 = zenoh + .ros2(Ros2Node::new("telemetry_gateway").namespace("/aimdb")) + .register::(); + +builder.configure::("robot.cmd_vel", |reg| { + reg.buffer(BufferCfg::SingleLatest) + .with_remote_access() + .linked_from("ros2://cmd_vel") // CDR, as a ROS subscription + .linked_to_with("zenoh://telemetry/cmd_vel", Json); // JSON, plain Zenoh +}); +``` + +`Twist` is a plain Rust struct with `#[derive(RosMessage)]` and the type hash +ROS publishes. Its nested `Vector3` is another derived struct. The +`SingleLatest` buffer makes the record the robot's *current* command: the +newest value wins, `aimdb record get` can read it at any time, and a late +subscriber receives it first. An `SpmcRing` would keep a backlog instead, and +has no current value to serve. + +Start the gateway, a plain Zenoh subscriber and a watch, each in its own +terminal: + +```text +$ target/debug/examples/ros2_gateway +ros2_gateway: /cmd_vel -> robot.cmd_vel -> telemetry/cmd_vel +ros2_gateway: router tcp/127.0.0.1:7447, AimX unix:///tmp/aimdb-ros2-gateway.sock + +$ target/debug/examples/zenoh_sub 'telemetry/**' +zenoh_sub: telemetry/** on tcp/127.0.0.1:7447 + +$ aimdb watch robot.cmd_vel +📡 Watching record: robot.cmd_vel +``` + +`zenoh_sub` is a stand-in for Zenoh's `z_sub`; `z_sub -k 'telemetry/**'` +shows the same payloads. Now publish two Twists from ROS: + +```text +ros2 topic pub --times 2 -r 1 /cmd_vel geometry_msgs/msg/Twist \ + "{linear: {x: 0.5}, angular: {z: 0.3}}" +``` + +`aimdb watch` prints each update as the record holds it: + +```text +2026-11-07 21:40:32.223 | seq:1 | ts:8.379694 | {"angular":{"x":0.0,"y":0.0,"z":0.3},"linear":{"x":0.5,"y":0.0,"z":0.0}} +2026-11-07 21:40:33.223 | seq:2 | ts:9.380052 | {"angular":{"x":0.0,"y":0.0,"z":0.3},"linear":{"x":0.5,"y":0.0,"z":0.0}} +``` + +The Zenoh subscriber receives the JSON payload, with the fields in `.msg` +order: + +```text +telemetry/cmd_vel: {"linear":{"x":0.5,"y":0.0,"z":0.0},"angular":{"x":0.0,"y":0.0,"z":0.3}} +telemetry/cmd_vel: {"linear":{"x":0.5,"y":0.0,"z":0.0},"angular":{"x":0.0,"y":0.0,"z":0.3}} +``` + +A watch started after the last message still sees it, because AimX sends the +record's current value as a snapshot when a subscriber joins: + +```text +$ aimdb watch robot.cmd_vel --count 1 +📡 Watching record: robot.cmd_vel +2026-11-07 21:47:28.626 | seq:1 | ts:5.334474 | {"angular":{"x":0.0,"y":0.0,"z":0.3},"linear":{"x":0.5,"y":0.0,"z":0.0}} +📸 snapshot complete (1 record) +``` + +## 4. Command: turtlesim through goals + +```text +caller ──AimX──▶ turtle.goal "Stop" | {"GoTo": {"x": 8.0, "y": 8.0}} +/turtle1/pose ──▶ turtle.pose +turtle.goal + turtle.pose ──controller──▶ turtle.cmd_vel ──▶ /turtle1/cmd_vel +``` + +| Record | Type | Written by | Over AimX | +|---|---|---|---| +| `turtle.goal` | `Goal`: `Stop` or `GoTo { x, y }` | the caller | read and write | +| `turtle.pose` | turtlesim's `Pose` | turtlesim | read only | +| `turtle.cmd_vel` | `geometry_msgs/msg/Twist` | the controller | read only | + +The controller is a `transform_join` over the goal and the pose. It runs on +every pose turtlesim publishes, about 60 times a second. It turns toward the +target, drives once it roughly faces it, and sends one zero command when it +arrives within 0.05 or is told to `Stop`. Speeds are clamped to 2 units/s +and 2 rad/s, and targets to the arena. None of that is up to the caller. + +There are two write barriers between a caller and the motors: + +- **AimX policy.** Only `turtle.goal` is in the writable set. +- **AimDB's single-writer rule.** `turtle.cmd_vel` is a transform's output, + so AimDB refuses remote writes to it whatever the policy says. + +turtlesim's messages moved packages after Jazzy: Jazzy publishes +`turtlesim/msg/Pose`, Lyrical `turtlesim_msgs/msg/Pose`. The fields are the +same, but the type name, and with it the hash, are not, and a mismatch fails +silently. The example carries both types, chooses by `ROS_DISTRO` (Jazzy when +it is unset), and prints its choice. + +Start turtlesim and the pilot: + +```text +$ ros2 run turtlesim turtlesim_node +$ ROS_DISTRO=jazzy target/debug/examples/turtlesim_mcp +turtlesim_mcp: /turtle1/pose (turtlesim::msg::dds_::Pose_) -> controller -> /turtle1/cmd_vel +turtlesim_mcp: router tcp/127.0.0.1:7447, AimX unix:///tmp/aimdb-turtlesim.sock (turtle.goal writable) +``` + +### With the CLI + +```text +$ aimdb record set turtle.goal '{"GoTo": {"x": 8.0, "y": 8.0}}' +✅ Successfully set record: turtle.goal +``` + +The turtle turns, drives and stops. The pilot logs: + +```text +goal: GoTo { x: 8.0, y: 8.0 } +stopped at (7.97, 7.96) +``` + +`aimdb watch turtle.cmd_vel` shows the controller's commands as it works, +and `aimdb record get turtle.pose` shows where the turtle ended up: + +```text +$ aimdb record get turtle.pose +{ + "ts": 14011178691, + "value": { + "angular_velocity": 0.0, + "linear_velocity": 0.0, + "theta": 0.9307765, + "x": 7.9706445, + "y": 7.960569 + } +} +``` + +`aimdb record set turtle.goal '"Stop"'` stops it mid-route. A direct velocity +command is refused: + +```text +$ aimdb record set turtle.cmd_vel '{"linear":{"x":9.0,"y":0.0,"z":0.0},"angular":{"x":0.0,"y":0.0,"z":0.0}}' +Error: Server error: denied + Message: permission denied +``` + +### With Claude + +`aimdb-mcp` offers `set_record` only when started with `--allow-writes`. In +Claude Code: + +```text +claude mcp add aimdb -- aimdb-mcp --connect unix:///tmp/aimdb-turtlesim.sock --allow-writes +``` + +In Claude Desktop's `claude_desktop_config.json`: + +```json +{ + "mcpServers": { + "aimdb": { + "command": "aimdb-mcp", + "args": ["--connect", "unix:///tmp/aimdb-turtlesim.sock", "--allow-writes"] + } + } +} +``` + +Then ask, for example, *"Where is the turtle? Drive it to the top-left +corner, then trace a square with side 3."* Claude reads `turtle.pose` with +`get_record`, writes one `GoTo` per corner with `set_record`, and checks the +pose between corners. If it tries `turtle.cmd_vel`, the write comes back as +`permission denied`. + +## How it is tested + +`make ros2-interop DISTRO=jazzy` (and `lyrical`) builds an image with +`rmw_zenoh_cpp`, `geometry_msgs` and `turtlesim`, starts `rmw_zenohd`, and +checks: + +- the gateway republishes 3 of 3 `ros2 topic pub` messages as the exact JSON + above, in domain 42, `aimdb watch` sees all three, and a later watch gets + the last one as a snapshot; +- `turtlesim_mcp` drives the turtle from its spawn point to within 0.1 of + `(8, 8)` after one `GoTo`, and refuses a write to `turtle.cmd_vel`. + +See [AimDB and ROS 2](aimdb-and-ros2.md) for the type mapping and the +connector's options.