From 69715a15059c27e6a9a4bddeae02e8766cfce091 Mon Sep 17 00:00:00 2001 From: "K. Isom" Date: Sun, 18 Oct 2026 21:03:32 -0700 Subject: [PATCH] fix(ros2): rclrs 0.8 API + bytes payload mapping Rewrite tines-ros2/src/node.rs against the rclrs 0.8 API (Context:: default_from_env -> create_basic_executor -> create_node; type-inferred subscriptions; thiserror-based RclrsError) and switch payloads from std_msgs String to UInt8MultiArray: * live topics publish the raw Observation.bytes as data (no UTF-8 lossy); * inbound callbacks use msg.data verbatim; * receipts are a 32-byte UInt8MultiArray (peer[..16] ++ id[..16]); * supply the required MultiArrayLayout { dim: [], data_offset: 0 }. Bump ros_env 0.1 -> 0.3 so the crate's std_msgs pins rclrs 0.8's rosidl runtime 0.7 (was 0.6), and update Cargo.lock. Add #[allow(dead_code)] on ManagedTopic::subscriber (drop-guard for the rcl subscription handle). Keep the ROS2-free build green; no changes outside tines-ros2/ (and the Cargo.lock entry). --- Cargo.lock | 1 + tines-ros2/Cargo.toml | 9 ++- tines-ros2/src/node.rs | 143 +++++++++++++++++++++-------------------- 3 files changed, 84 insertions(+), 69 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 24c22af5..96621b65 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1498,6 +1498,7 @@ name = "tines-ros2" version = "0.3.0" dependencies = [ "rclrs", + "ros-env", "tines-bt", "tines-core", "tines-store", diff --git a/tines-ros2/Cargo.toml b/tines-ros2/Cargo.toml index 96c9a1a9..c17f9c2f 100644 --- a/tines-ros2/Cargo.toml +++ b/tines-ros2/Cargo.toml @@ -17,12 +17,19 @@ categories = ["science::robotics", "network-programming"] # and are tested without a ROS2 toolchain. Enable `--features ros2` to compile # the rclrs-backed `TinesNode`. default = [] -ros2 = ["dep:rclrs"] +ros2 = ["dep:rclrs", "dep:ros-env"] [dependencies] tines-core = { path = "../tines-core", version = "0.3.0" } tines-store = { path = "../tines-store", version = "0.3.0" } tines-bt = { path = "../tines-bt", version = "0.3.0" } rclrs = { version = "0.8", optional = true } +# ros_env provides the ROS 2 message types (std_msgs, including +# UInt8MultiArray) by include!()ing the generated crates from +# AMENT_PREFIX_PATH; only needed behind the `ros2` feature alongside rclrs. +# Pinned to 0.3 so it shares rclrs 0.8's rosidl_runtime_rs 0.7 rather than the +# 0.6 that ros_env 0.1 pulls in. (The crates.io package is `ros-env`; Cargo maps +# it to the `ros_env` crate id.) +ros-env = { version = "0.3", optional = true } [dev-dependencies] diff --git a/tines-ros2/src/node.rs b/tines-ros2/src/node.rs index c41624c0..7b408d46 100644 --- a/tines-ros2/src/node.rs +++ b/tines-ros2/src/node.rs @@ -5,11 +5,14 @@ //! It bridges the two stacks in both directions: //! //! * **tines → ROS2**: each distinct live tines topic is published to its -//! hex-escaped ROS2 topic (see [`crate::topics::topic_name`]), and every +//! hex-escaped ROS2 topic (see [`crate::topics::topic_name`]) carrying the +//! raw observation payload as a `UInt8MultiArray` (`data: Vec`), and every //! [`tines_core::ConfidenceEvent`] the exchange emits is published to -//! [`RECEIPTS_TOPIC`]. +//! [`RECEIPTS_TOPIC`] as a 32-byte `UInt8MultiArray` (`peer[..16] ++ +//! id[..16]`). //! * **ROS2 → tines**: inbound messages on a topic are observed back into the -//! tines node under the original (decoded) topic. +//! tines node under the original (decoded) topic, the payload being the +//! `UInt8MultiArray.data` bytes verbatim. //! //! Topic creation is lazy: a ROS2 publisher/subscriber pair is created the //! first time a topic is seen; the total number of managed topics is bounded by @@ -20,25 +23,30 @@ //! toolchain (and `rclrs`) to build. The default, ROS2-free build of the crate //! never touches this code, so the crate is testable without ROS2 installed. //! -//! The rclrs calls below follow the rclrs 0.8 API (see the rclrs tutorial): -//! the global ROS2 context is set by [`rclrs::init`] (the caller's -//! responsibility, done before [`TinesNode::new`]); `Node::new(name, ns)` uses -//! it; publishers/subscriptions are created with `create_publisher`/ -//! `create_subscription`, each taking a `QosProfile`. The exact generic -//! arity of those two calls (`` for a publisher, `` for a -//! subscription) and the `QosProfile` builder methods track rclrs 0.8; verify -//! against the installed `rclrs` version before relying on them. +//! The rclrs calls below follow the rclrs 0.8 API: +//! * the global ROS2 context is created via [`rclrs::Context::default_from_env`], +//! then an [`rclrs::Executor`] is created from it and the node is created with +//! [`rclrs::NodeState::create_node`]; +//! * publishers are created with [`rclrs::NodeState::create_publisher`] and +//! subscriptions with [`rclrs::NodeState::create_subscription`], whose +//! callback argument's type is inferred (no generic, no QoS arg); the topic +//! is passed as the options argument, which defaults to `QOS_PROFILE_DEFAULT`. +//! * messages live in the `ros_env` crate (see the `ros_env` dependency), not +//! under `rclrs`. +//! +//! The `Context`/`Executor` are created per [`TinesNode::new`]; the node is +//! retained by [`TinesNode`] and owns the shared context, so no caller-side +//! [`rclrs::init`] is required (the pre-0.8 `rclrs::init` global is gone). use std::collections::{BTreeSet, HashMap}; use std::sync::mpsc::channel; -use rclrs::prelude::{Node as RclNode, Publisher, QosProfile, Subscription}; -use rclrs::std_msgs::msg::String as StrMsg; -use rclrs::RclError; +use rclrs::{Context, CreateBasicExecutor, Node as RclNode, Publisher, RclrsError, Subscription}; +use ros_env::std_msgs::msg::{MultiArrayLayout, UInt8MultiArray}; use tines_bt::{BtTransport, Radio}; use tines_core::{ - Node as TinesNodeCore, NodeId, Observation, ObservationId, Store, Tick, Transport, + Node as TinesNodeCore, NodeId, Observation, ObservationId, Store, Tick, }; use tines_store::LogStore; @@ -54,42 +62,31 @@ type InboundMsg = (Vec, Vec); /// takes a `&str` topic name, unlike the arbitrary-byte [`crate::RECEIPTS_TOPIC`]. const RECEIPTS_TOPIC: &str = "/tines/receipts"; -/// QoS for live tines-topic payloads: best-effort, so a slow consumer never -/// stalls the node (sensor-style data is continuously refreshed anyway). -fn topic_qos() -> QosProfile { - QosProfile::default().best_effort().keep_last(128) -} - -/// QoS for receipts: reliable + transient-local, so a late-joining consumer -/// still receives the receipts emitted while it was disconnected. -fn receipt_qos() -> QosProfile { - QosProfile::default() - .reliable() - .transient_local() - .keep_last(256) -} - /// A managed ROS2 topic: the tines bytes it maps back to, its publisher, its /// subscription, and the id of the observation last published (to avoid /// re-publishing an unchanged value every poll). struct ManagedTopic { tines_topic: Vec, - publisher: Option>, - subscriber: Option>, + publisher: Option>, + // The subscription handle must stay alive for the node's lifetime: rclrs 0.8 + // `Subscription` owns the underlying rcl handle, so dropping it would tear the + // subscription down. We never read its fields, hence the allow. + #[allow(dead_code)] + subscriber: Option>, last_pub: Option, } /// Errors constructing a [`TinesNode`]. #[derive(Debug)] pub enum TinesNodeError { - /// The underlying rclrs call failed (ROS2 context, QoS, topic creation). - Rcl(RclError), + /// The underlying rclrs call failed (ROS2 context, topic creation). + Rcl(RclrsError), /// The [`LogStore`] backend rejected the open/initialization. Store(tines_core::StoreError), } -impl From for TinesNodeError { - fn from(err: RclError) -> Self { +impl From for TinesNodeError { + fn from(err: RclrsError) -> Self { TinesNodeError::Rcl(err) } } @@ -112,7 +109,7 @@ pub struct TinesNode { /// ROS2 topics currently managed (ROS2 topic name → bookkeeping). managed: HashMap, /// Receipt publisher for confidence events. - receipt_pub: Publisher, + receipt_pub: Publisher, /// Channel from subscription callbacks back to this node. inbound_rx: std::sync::mpsc::Receiver, /// Cloned per subscription so callbacks can route inbound bytes. @@ -130,9 +127,9 @@ impl TinesNode { /// * `first_seq` — the next tines observation sequence to use. /// * `topic_cap` — the maximum number of ROS2 topics to manage at once. /// - /// The ROS2 context must already be initialized via [`rclrs::init`] (the - /// caller's responsibility). The store's anchor tick is `0`; ages then - /// advance under the `Tick` values passed to [`TinesNode::poll`]. + /// The ROS2 context is created internally via [`rclrs::Context::default_from_env`]; + /// the store's anchor tick is `0`; ages then advance under the `Tick` + /// values passed to [`TinesNode::poll`]. pub fn new( me: NodeId, first_seq: u64, @@ -141,14 +138,19 @@ impl TinesNode { radio: impl Radio + 'static, topic_cap: usize, ) -> Result { - // rclrs uses the global context set by `rclrs::init`. - let node = RclNode::new("tines", "")?; + // rclrs 0.8: create the global context, then a basic executor, then the + // node through that executor (the pre-0.8 `rclrs::init` + + // `Node::new(name, ns)` shape is gone). The node owns the shared + // context, so no caller-side init is required. + let context = Context::default_from_env()?; + let executor = context.create_basic_executor(); + let node = executor.create_node("tines")?; let store = LogStore::new(log_path, 0, first_seq)?; let transport = BtTransport::new(me, peer, radio); let tines = TinesNodeCore::new(me, first_seq, store, transport); - let receipt_pub = node.create_publisher::(RECEIPTS_TOPIC, receipt_qos())?; + let receipt_pub = node.create_publisher::(RECEIPTS_TOPIC)?; let (inbound_tx, inbound_rx) = channel::(); Ok(Self { @@ -185,7 +187,7 @@ impl TinesNode { Ok(stored) } - /// Observe inbound ROS2 observations into the tines node, skipping anything + /// Consume inbound ROS2 observations into the tines node, skipping anything /// oversized for tines to reject. fn drain_inbound(&mut self, now: Tick) { while let Ok((topic, payload)) = self.inbound_rx.try_recv() { @@ -200,6 +202,10 @@ impl TinesNode { /// Lazily create a ROS2 publisher/subscriber pair for `tines_topic`, bounded /// by [`TinesNode::new`]'s `topic_cap`. + /// + /// The subscription callback carries a cloned channel sender and a copy of + /// the original topic bytes (not `self`) so it can outlive this borrow and + /// run on the rclrs dispatch thread. fn ensure(&mut self, tines_topic: &[u8]) -> Result<(), TinesNodeError> { let name = topic_name(tines_topic); if self.managed.contains_key(&name) { @@ -213,16 +219,18 @@ impl TinesNode { // original topic bytes — not `self` — so it can outlive this borrow. let inbound_tx = self.inbound_tx.clone(); let tines_topic_cb = tines_topic.to_vec(); - let subscriber = self.node.create_subscription::( - &name, - topic_qos(), - move |msg: &StrMsg| { - let _ = inbound_tx.send((tines_topic_cb.clone(), msg.data.as_bytes().to_vec())); - }, - )?; + let subscriber = self + .node + .create_subscription( + &name, + move |msg: UInt8MultiArray| { + // `msg.data` is the raw payload bytes verbatim. + let _ = inbound_tx.send((tines_topic_cb.clone(), msg.data.clone())); + }, + )?; let publisher = self .node - .create_publisher::(&name, topic_qos())?; + .create_publisher::(&name)?; self.managed.insert( name, @@ -241,9 +249,8 @@ impl TinesNode { // Distinct live topics, via the store's topic iterator. let mut topics: BTreeSet> = BTreeSet::new(); self.tines.store().for_each_topic(&[], now, &mut |obs| { - topics.insert(obs.topic.as_slice().to_vec()) + topics.insert(obs.topic.as_slice().to_vec()); }); - // Create publishers for any topic we don't yet manage (cap-bounded in // [`Self::ensure`]). for tines_topic in topics { @@ -290,8 +297,10 @@ impl TinesNode { if topic.last_pub == Some(obs.id()) { return Ok(()); } - let payload = String::from_utf8_lossy(&obs.payload).into_owned(); - let msg = StrMsg { data: payload }; + // Publish the raw payload bytes as a `UInt8MultiArray`. + let data = obs.payload.to_vec(); + let msg = + UInt8MultiArray { data, layout: MultiArrayLayout { dim: vec![], data_offset: 0 } }; if let Some(publisher) = topic.publisher.as_mut() { let _ = publisher.publish(&msg); topic.last_pub = Some(obs.id()); @@ -313,22 +322,20 @@ impl TinesNode { } /// Publish every pending confidence event to [`RECEIPTS_TOPIC`]. + /// + /// Each receipt is a 32-byte `UInt8MultiArray` = `peer[..16] ++ id[..16]`, + /// the raw node ids (no hex encoding). fn publish_receipts(&mut self) { let events = self.tines.drain_events(); for ev in events.iter() { - let payload = format!("{}:{}", hex16(ev.peer), hex16(ev.id)); - let msg = StrMsg { data: payload }; + let mut data = [0u8; 32]; + data[..16].copy_from_slice(&ev.peer); + data[16..32].copy_from_slice(&ev.id); + let msg = UInt8MultiArray { + data: data.to_vec(), + layout: MultiArrayLayout { dim: vec![], data_offset: 0 }, + }; let _ = self.receipt_pub.publish(&msg); } } } - -/// Render a fixed-width id as lowercase hex (no allocator needed). -fn hex16(id: &[u8; 16]) -> String { - use core::fmt::Write; - let mut s = String::with_capacity(32); - for byte in id { - let _ = write!(s, "{byte:02x}"); - } - s -}