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).
This commit is contained in:
Generated
+1
@@ -1498,6 +1498,7 @@ name = "tines-ros2"
|
|||||||
version = "0.3.0"
|
version = "0.3.0"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
"rclrs",
|
"rclrs",
|
||||||
|
"ros-env",
|
||||||
"tines-bt",
|
"tines-bt",
|
||||||
"tines-core",
|
"tines-core",
|
||||||
"tines-store",
|
"tines-store",
|
||||||
|
|||||||
@@ -17,12 +17,19 @@ categories = ["science::robotics", "network-programming"]
|
|||||||
# and are tested without a ROS2 toolchain. Enable `--features ros2` to compile
|
# and are tested without a ROS2 toolchain. Enable `--features ros2` to compile
|
||||||
# the rclrs-backed `TinesNode`.
|
# the rclrs-backed `TinesNode`.
|
||||||
default = []
|
default = []
|
||||||
ros2 = ["dep:rclrs"]
|
ros2 = ["dep:rclrs", "dep:ros-env"]
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
tines-core = { path = "../tines-core", version = "0.3.0" }
|
tines-core = { path = "../tines-core", version = "0.3.0" }
|
||||||
tines-store = { path = "../tines-store", version = "0.3.0" }
|
tines-store = { path = "../tines-store", version = "0.3.0" }
|
||||||
tines-bt = { path = "../tines-bt", version = "0.3.0" }
|
tines-bt = { path = "../tines-bt", version = "0.3.0" }
|
||||||
rclrs = { version = "0.8", optional = true }
|
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]
|
[dev-dependencies]
|
||||||
|
|||||||
+72
-65
@@ -5,11 +5,14 @@
|
|||||||
//! It bridges the two stacks in both directions:
|
//! It bridges the two stacks in both directions:
|
||||||
//!
|
//!
|
||||||
//! * **tines → ROS2**: each distinct live tines topic is published to its
|
//! * **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<u8>`), and every
|
||||||
//! [`tines_core::ConfidenceEvent`] the exchange emits is published to
|
//! [`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
|
//! * **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
|
//! 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
|
//! 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
|
//! 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.
|
//! 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 rclrs calls below follow the rclrs 0.8 API:
|
||||||
//! the global ROS2 context is set by [`rclrs::init`] (the caller's
|
//! * the global ROS2 context is created via [`rclrs::Context::default_from_env`],
|
||||||
//! responsibility, done before [`TinesNode::new`]); `Node::new(name, ns)` uses
|
//! then an [`rclrs::Executor`] is created from it and the node is created with
|
||||||
//! it; publishers/subscriptions are created with `create_publisher`/
|
//! [`rclrs::NodeState::create_node`];
|
||||||
//! `create_subscription`, each taking a `QosProfile`. The exact generic
|
//! * publishers are created with [`rclrs::NodeState::create_publisher`] and
|
||||||
//! arity of those two calls (`<Msg, _>` for a publisher, `<Msg, _, _>` for a
|
//! subscriptions with [`rclrs::NodeState::create_subscription`], whose
|
||||||
//! subscription) and the `QosProfile` builder methods track rclrs 0.8; verify
|
//! callback argument's type is inferred (no generic, no QoS arg); the topic
|
||||||
//! against the installed `rclrs` version before relying on them.
|
//! 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::collections::{BTreeSet, HashMap};
|
||||||
use std::sync::mpsc::channel;
|
use std::sync::mpsc::channel;
|
||||||
|
|
||||||
use rclrs::prelude::{Node as RclNode, Publisher, QosProfile, Subscription};
|
use rclrs::{Context, CreateBasicExecutor, Node as RclNode, Publisher, RclrsError, Subscription};
|
||||||
use rclrs::std_msgs::msg::String as StrMsg;
|
use ros_env::std_msgs::msg::{MultiArrayLayout, UInt8MultiArray};
|
||||||
use rclrs::RclError;
|
|
||||||
|
|
||||||
use tines_bt::{BtTransport, Radio};
|
use tines_bt::{BtTransport, Radio};
|
||||||
use tines_core::{
|
use tines_core::{
|
||||||
Node as TinesNodeCore, NodeId, Observation, ObservationId, Store, Tick, Transport,
|
Node as TinesNodeCore, NodeId, Observation, ObservationId, Store, Tick,
|
||||||
};
|
};
|
||||||
use tines_store::LogStore;
|
use tines_store::LogStore;
|
||||||
|
|
||||||
@@ -54,42 +62,31 @@ type InboundMsg = (Vec<u8>, Vec<u8>);
|
|||||||
/// takes a `&str` topic name, unlike the arbitrary-byte [`crate::RECEIPTS_TOPIC`].
|
/// takes a `&str` topic name, unlike the arbitrary-byte [`crate::RECEIPTS_TOPIC`].
|
||||||
const RECEIPTS_TOPIC: &str = "/tines/receipts";
|
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
|
/// 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
|
/// subscription, and the id of the observation last published (to avoid
|
||||||
/// re-publishing an unchanged value every poll).
|
/// re-publishing an unchanged value every poll).
|
||||||
struct ManagedTopic {
|
struct ManagedTopic {
|
||||||
tines_topic: Vec<u8>,
|
tines_topic: Vec<u8>,
|
||||||
publisher: Option<Publisher<StrMsg>>,
|
publisher: Option<Publisher<UInt8MultiArray>>,
|
||||||
subscriber: Option<Subscription<StrMsg>>,
|
// 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<Subscription<UInt8MultiArray>>,
|
||||||
last_pub: Option<ObservationId>,
|
last_pub: Option<ObservationId>,
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Errors constructing a [`TinesNode`].
|
/// Errors constructing a [`TinesNode`].
|
||||||
#[derive(Debug)]
|
#[derive(Debug)]
|
||||||
pub enum TinesNodeError {
|
pub enum TinesNodeError {
|
||||||
/// The underlying rclrs call failed (ROS2 context, QoS, topic creation).
|
/// The underlying rclrs call failed (ROS2 context, topic creation).
|
||||||
Rcl(RclError),
|
Rcl(RclrsError),
|
||||||
/// The [`LogStore`] backend rejected the open/initialization.
|
/// The [`LogStore`] backend rejected the open/initialization.
|
||||||
Store(tines_core::StoreError),
|
Store(tines_core::StoreError),
|
||||||
}
|
}
|
||||||
|
|
||||||
impl From<RclError> for TinesNodeError {
|
impl From<RclrsError> for TinesNodeError {
|
||||||
fn from(err: RclError) -> Self {
|
fn from(err: RclrsError) -> Self {
|
||||||
TinesNodeError::Rcl(err)
|
TinesNodeError::Rcl(err)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -112,7 +109,7 @@ pub struct TinesNode {
|
|||||||
/// ROS2 topics currently managed (ROS2 topic name → bookkeeping).
|
/// ROS2 topics currently managed (ROS2 topic name → bookkeeping).
|
||||||
managed: HashMap<String, ManagedTopic>,
|
managed: HashMap<String, ManagedTopic>,
|
||||||
/// Receipt publisher for confidence events.
|
/// Receipt publisher for confidence events.
|
||||||
receipt_pub: Publisher<StrMsg>,
|
receipt_pub: Publisher<UInt8MultiArray>,
|
||||||
/// Channel from subscription callbacks back to this node.
|
/// Channel from subscription callbacks back to this node.
|
||||||
inbound_rx: std::sync::mpsc::Receiver<InboundMsg>,
|
inbound_rx: std::sync::mpsc::Receiver<InboundMsg>,
|
||||||
/// Cloned per subscription so callbacks can route inbound bytes.
|
/// Cloned per subscription so callbacks can route inbound bytes.
|
||||||
@@ -130,9 +127,9 @@ impl TinesNode {
|
|||||||
/// * `first_seq` — the next tines observation sequence to use.
|
/// * `first_seq` — the next tines observation sequence to use.
|
||||||
/// * `topic_cap` — the maximum number of ROS2 topics to manage at once.
|
/// * `topic_cap` — the maximum number of ROS2 topics to manage at once.
|
||||||
///
|
///
|
||||||
/// The ROS2 context must already be initialized via [`rclrs::init`] (the
|
/// The ROS2 context is created internally via [`rclrs::Context::default_from_env`];
|
||||||
/// caller's responsibility). The store's anchor tick is `0`; ages then
|
/// the store's anchor tick is `0`; ages then advance under the `Tick`
|
||||||
/// advance under the `Tick` values passed to [`TinesNode::poll`].
|
/// values passed to [`TinesNode::poll`].
|
||||||
pub fn new(
|
pub fn new(
|
||||||
me: NodeId,
|
me: NodeId,
|
||||||
first_seq: u64,
|
first_seq: u64,
|
||||||
@@ -141,14 +138,19 @@ impl TinesNode {
|
|||||||
radio: impl Radio + 'static,
|
radio: impl Radio + 'static,
|
||||||
topic_cap: usize,
|
topic_cap: usize,
|
||||||
) -> Result<Self, TinesNodeError> {
|
) -> Result<Self, TinesNodeError> {
|
||||||
// rclrs uses the global context set by `rclrs::init`.
|
// rclrs 0.8: create the global context, then a basic executor, then the
|
||||||
let node = RclNode::new("tines", "")?;
|
// 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 store = LogStore::new(log_path, 0, first_seq)?;
|
||||||
let transport = BtTransport::new(me, peer, radio);
|
let transport = BtTransport::new(me, peer, radio);
|
||||||
let tines = TinesNodeCore::new(me, first_seq, store, transport);
|
let tines = TinesNodeCore::new(me, first_seq, store, transport);
|
||||||
|
|
||||||
let receipt_pub = node.create_publisher::<StrMsg, _>(RECEIPTS_TOPIC, receipt_qos())?;
|
let receipt_pub = node.create_publisher::<UInt8MultiArray>(RECEIPTS_TOPIC)?;
|
||||||
let (inbound_tx, inbound_rx) = channel::<InboundMsg>();
|
let (inbound_tx, inbound_rx) = channel::<InboundMsg>();
|
||||||
|
|
||||||
Ok(Self {
|
Ok(Self {
|
||||||
@@ -185,7 +187,7 @@ impl TinesNode {
|
|||||||
Ok(stored)
|
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.
|
/// oversized for tines to reject.
|
||||||
fn drain_inbound(&mut self, now: Tick) {
|
fn drain_inbound(&mut self, now: Tick) {
|
||||||
while let Ok((topic, payload)) = self.inbound_rx.try_recv() {
|
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
|
/// Lazily create a ROS2 publisher/subscriber pair for `tines_topic`, bounded
|
||||||
/// by [`TinesNode::new`]'s `topic_cap`.
|
/// 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> {
|
fn ensure(&mut self, tines_topic: &[u8]) -> Result<(), TinesNodeError> {
|
||||||
let name = topic_name(tines_topic);
|
let name = topic_name(tines_topic);
|
||||||
if self.managed.contains_key(&name) {
|
if self.managed.contains_key(&name) {
|
||||||
@@ -213,16 +219,18 @@ impl TinesNode {
|
|||||||
// original topic bytes — not `self` — so it can outlive this borrow.
|
// original topic bytes — not `self` — so it can outlive this borrow.
|
||||||
let inbound_tx = self.inbound_tx.clone();
|
let inbound_tx = self.inbound_tx.clone();
|
||||||
let tines_topic_cb = tines_topic.to_vec();
|
let tines_topic_cb = tines_topic.to_vec();
|
||||||
let subscriber = self.node.create_subscription::<StrMsg, _, _>(
|
let subscriber = self
|
||||||
|
.node
|
||||||
|
.create_subscription(
|
||||||
&name,
|
&name,
|
||||||
topic_qos(),
|
move |msg: UInt8MultiArray| {
|
||||||
move |msg: &StrMsg| {
|
// `msg.data` is the raw payload bytes verbatim.
|
||||||
let _ = inbound_tx.send((tines_topic_cb.clone(), msg.data.as_bytes().to_vec()));
|
let _ = inbound_tx.send((tines_topic_cb.clone(), msg.data.clone()));
|
||||||
},
|
},
|
||||||
)?;
|
)?;
|
||||||
let publisher = self
|
let publisher = self
|
||||||
.node
|
.node
|
||||||
.create_publisher::<StrMsg, _>(&name, topic_qos())?;
|
.create_publisher::<UInt8MultiArray>(&name)?;
|
||||||
|
|
||||||
self.managed.insert(
|
self.managed.insert(
|
||||||
name,
|
name,
|
||||||
@@ -241,9 +249,8 @@ impl TinesNode {
|
|||||||
// Distinct live topics, via the store's topic iterator.
|
// Distinct live topics, via the store's topic iterator.
|
||||||
let mut topics: BTreeSet<Vec<u8>> = BTreeSet::new();
|
let mut topics: BTreeSet<Vec<u8>> = BTreeSet::new();
|
||||||
self.tines.store().for_each_topic(&[], now, &mut |obs| {
|
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
|
// Create publishers for any topic we don't yet manage (cap-bounded in
|
||||||
// [`Self::ensure`]).
|
// [`Self::ensure`]).
|
||||||
for tines_topic in topics {
|
for tines_topic in topics {
|
||||||
@@ -290,8 +297,10 @@ impl TinesNode {
|
|||||||
if topic.last_pub == Some(obs.id()) {
|
if topic.last_pub == Some(obs.id()) {
|
||||||
return Ok(());
|
return Ok(());
|
||||||
}
|
}
|
||||||
let payload = String::from_utf8_lossy(&obs.payload).into_owned();
|
// Publish the raw payload bytes as a `UInt8MultiArray`.
|
||||||
let msg = StrMsg { data: payload };
|
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() {
|
if let Some(publisher) = topic.publisher.as_mut() {
|
||||||
let _ = publisher.publish(&msg);
|
let _ = publisher.publish(&msg);
|
||||||
topic.last_pub = Some(obs.id());
|
topic.last_pub = Some(obs.id());
|
||||||
@@ -313,22 +322,20 @@ impl TinesNode {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Publish every pending confidence event to [`RECEIPTS_TOPIC`].
|
/// 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) {
|
fn publish_receipts(&mut self) {
|
||||||
let events = self.tines.drain_events();
|
let events = self.tines.drain_events();
|
||||||
for ev in events.iter() {
|
for ev in events.iter() {
|
||||||
let payload = format!("{}:{}", hex16(ev.peer), hex16(ev.id));
|
let mut data = [0u8; 32];
|
||||||
let msg = StrMsg { data: payload };
|
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);
|
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
|
|
||||||
}
|
|
||||||
|
|||||||
Reference in New Issue
Block a user