335 lines
13 KiB
Rust
335 lines
13 KiB
Rust
//! rclrs-backed ROS2 node for tines (feature `ros2`).
|
|
//!
|
|
//! [`TinesNode`] is a self-contained ROS2 node that owns a tines
|
|
//! [`tines_core::Node`] over a [`tines_store::LogStore`] and a [`tines_bt::BtTransport`].
|
|
//! 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
|
|
//! [`tines_core::ConfidenceEvent`] the exchange emits is published to
|
|
//! [`RECEIPTS_TOPIC`].
|
|
//! * **ROS2 → tines**: inbound messages on a topic are observed back into the
|
|
//! tines node under the original (decoded) topic.
|
|
//!
|
|
//! 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
|
|
//! [`TinesNode::new`]'s `topic_cap` and by the number of topics actually live
|
|
//! in the store.
|
|
//!
|
|
//! This module is compiled only with `--features ros2`; it requires a ROS2
|
|
//! 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 (`<Msg, _>` for a publisher, `<Msg, _, _>` for a
|
|
//! subscription) and the `QosProfile` builder methods track rclrs 0.8; verify
|
|
//! against the installed `rclrs` version before relying on them.
|
|
|
|
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 tines_bt::{BtTransport, Radio};
|
|
use tines_core::{
|
|
Node as TinesNodeCore, NodeId, Observation, ObservationId, Store, Tick, Transport,
|
|
};
|
|
use tines_store::LogStore;
|
|
|
|
use crate::topics::topic_name;
|
|
|
|
/// Inbound (topic bytes, payload) delivered from a subscription callback on the
|
|
/// rclrs dispatch thread to the node's single owner task.
|
|
type InboundMsg = (Vec<u8>, Vec<u8>);
|
|
|
|
/// ROS2 topic carrying confidence events, the literal `/tines/receipts`
|
|
/// (hex-escaping is a no-op here since the bytes are already valid ROS2 name
|
|
/// characters). This is a `&str` because ROS2 publisher/subscription creation
|
|
/// 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<u8>,
|
|
publisher: Option<Publisher<StrMsg>>,
|
|
subscriber: Option<Subscription<StrMsg>>,
|
|
last_pub: Option<ObservationId>,
|
|
}
|
|
|
|
/// Errors constructing a [`TinesNode`].
|
|
#[derive(Debug)]
|
|
pub enum TinesNodeError {
|
|
/// The underlying rclrs call failed (ROS2 context, QoS, topic creation).
|
|
Rcl(RclError),
|
|
/// The [`LogStore`] backend rejected the open/initialization.
|
|
Store(tines_core::StoreError),
|
|
}
|
|
|
|
impl From<RclError> for TinesNodeError {
|
|
fn from(err: RclError) -> Self {
|
|
TinesNodeError::Rcl(err)
|
|
}
|
|
}
|
|
|
|
impl From<tines_core::StoreError> for TinesNodeError {
|
|
fn from(err: tines_core::StoreError) -> Self {
|
|
TinesNodeError::Store(err)
|
|
}
|
|
}
|
|
|
|
/// A self-contained ROS2 node bridging tines and ROS2.
|
|
///
|
|
/// Construct with [`TinesNode::new`], then drive it from your run loop with
|
|
/// [`TinesNode::poll`]. Inbound ROS2 observations arrive on an internal
|
|
/// channel and are consumed on the next [`TinesNode::poll`], so the node is not
|
|
/// thread-safe by itself — drive it from a single task.
|
|
pub struct TinesNode {
|
|
node: RclNode,
|
|
tines: TinesNodeCore<LogStore, BtTransport>,
|
|
/// ROS2 topics currently managed (ROS2 topic name → bookkeeping).
|
|
managed: HashMap<String, ManagedTopic>,
|
|
/// Receipt publisher for confidence events.
|
|
receipt_pub: Publisher<StrMsg>,
|
|
/// Channel from subscription callbacks back to this node.
|
|
inbound_rx: std::sync::mpsc::Receiver<InboundMsg>,
|
|
/// Cloned per subscription so callbacks can route inbound bytes.
|
|
inbound_tx: std::sync::mpsc::Sender<InboundMsg>,
|
|
/// Upper bound on the number of managed ROS2 topics.
|
|
topic_cap: usize,
|
|
}
|
|
|
|
impl TinesNode {
|
|
/// Initialize a node for local id `me` over a [`LogStore`] and a
|
|
/// [`BtTransport`].
|
|
///
|
|
/// * `log_path` — where the backing [`LogStore`] lives (created if missing).
|
|
/// * `peer` / `radio` — the BLE connection this node exchanges over.
|
|
/// * `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`].
|
|
pub fn new(
|
|
me: NodeId,
|
|
first_seq: u64,
|
|
log_path: &std::path::Path,
|
|
peer: NodeId,
|
|
radio: impl Radio + 'static,
|
|
topic_cap: usize,
|
|
) -> Result<Self, TinesNodeError> {
|
|
// rclrs uses the global context set by `rclrs::init`.
|
|
let node = RclNode::new("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::<StrMsg, _>(RECEIPTS_TOPIC, receipt_qos())?;
|
|
let (inbound_tx, inbound_rx) = channel::<InboundMsg>();
|
|
|
|
Ok(Self {
|
|
node,
|
|
tines,
|
|
managed: HashMap::new(),
|
|
receipt_pub,
|
|
inbound_rx,
|
|
inbound_tx,
|
|
topic_cap,
|
|
})
|
|
}
|
|
|
|
/// The local tines node id this node was built with.
|
|
pub fn id(&self) -> NodeId {
|
|
self.tines.id()
|
|
}
|
|
|
|
/// The current in-memory count of managed ROS2 topics.
|
|
pub fn managed_topic_count(&self) -> usize {
|
|
self.managed.len()
|
|
}
|
|
|
|
/// One bridge tick: consume inbound ROS2 observations, drive the tines
|
|
/// exchange, publish live topics, and publish any confidence events.
|
|
///
|
|
/// Returns the number of *new* observations stored this round (from
|
|
/// [`TinesNodeCore::poll`]).
|
|
pub fn poll(&mut self, now: Tick) -> Result<usize, TinesNodeError> {
|
|
self.drain_inbound(now);
|
|
let stored = self.tines.poll(now);
|
|
self.publish_live_topics(now)?;
|
|
self.publish_receipts();
|
|
Ok(stored)
|
|
}
|
|
|
|
/// Observe 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() {
|
|
if payload.len() > tines_core::consts::MAX_PAYLOAD
|
|
|| topic.len() > tines_core::consts::MAX_TOPIC
|
|
{
|
|
continue; // oversized; tines would reject it anyway
|
|
}
|
|
let _ = self.tines.observe(&topic, &payload, None, now);
|
|
}
|
|
}
|
|
|
|
/// Lazily create a ROS2 publisher/subscriber pair for `tines_topic`, bounded
|
|
/// by [`TinesNode::new`]'s `topic_cap`.
|
|
fn ensure(&mut self, tines_topic: &[u8]) -> Result<(), TinesNodeError> {
|
|
let name = topic_name(tines_topic);
|
|
if self.managed.contains_key(&name) {
|
|
return Ok(());
|
|
}
|
|
if self.managed.len() >= self.topic_cap {
|
|
return Ok(()); // cap reached; bound live-topic creation
|
|
}
|
|
|
|
// The subscription callback owns a channel sender and a copy of the
|
|
// 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::<StrMsg, _, _>(
|
|
&name,
|
|
topic_qos(),
|
|
move |msg: &StrMsg| {
|
|
let _ = inbound_tx.send((tines_topic_cb.clone(), msg.data.as_bytes().to_vec()));
|
|
},
|
|
)?;
|
|
let publisher = self
|
|
.node
|
|
.create_publisher::<StrMsg, _>(&name, topic_qos())?;
|
|
|
|
self.managed.insert(
|
|
name,
|
|
ManagedTopic {
|
|
tines_topic: tines_topic.to_vec(),
|
|
publisher: Some(publisher),
|
|
subscriber: Some(subscriber),
|
|
last_pub: None,
|
|
},
|
|
);
|
|
Ok(())
|
|
}
|
|
|
|
/// Publish every distinct topic currently live in the store.
|
|
fn publish_live_topics(&mut self, now: Tick) -> Result<(), TinesNodeError> {
|
|
// Distinct live topics, via the store's topic iterator.
|
|
let mut topics: BTreeSet<Vec<u8>> = BTreeSet::new();
|
|
self.tines.store().for_each_topic(&[], now, &mut |obs| {
|
|
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 {
|
|
let name = topic_name(&tines_topic);
|
|
if self.managed.contains_key(&name) {
|
|
continue;
|
|
}
|
|
self.ensure(&tines_topic)?;
|
|
}
|
|
|
|
// Publish the latest observation for every managed topic. We only need
|
|
// each topic's `tines_topic` bytes to look up its latest observation,
|
|
// so clone them (and the name) into owned locals *before* calling
|
|
// `publish_one`. That drops the borrow of `self.managed` up front —
|
|
// holding `&mut self.managed` across the `&mut self` call is the
|
|
// sibling of the E0502 the reviewer flagged.
|
|
let managed: Vec<(String, Vec<u8>)> = self
|
|
.managed
|
|
.iter()
|
|
.map(|(name, t)| (name.clone(), t.tines_topic.clone()))
|
|
.collect();
|
|
for (name, tines_topic) in managed {
|
|
self.publish_one(&tines_topic, name, now)?;
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/// Publish the latest observation for a single managed topic, unless its
|
|
/// value is unchanged since the last publish (to avoid a busy loop).
|
|
fn publish_one(
|
|
&mut self,
|
|
tines_topic: &[u8],
|
|
name: String,
|
|
now: Tick,
|
|
) -> Result<(), TinesNodeError> {
|
|
// `latest_obs` takes `&self`, but `topic` below would hold a live
|
|
// `&mut self.managed` — the E0502 the reviewer flagged. Collect the
|
|
// latest observation on an immutable borrow that ends here, then take
|
|
// `&mut self.managed` for only the publish + bookkeeping that follows.
|
|
let Some(obs) = self.latest_obs(tines_topic, now) else {
|
|
return Ok(());
|
|
};
|
|
if let Some(topic) = self.managed.get_mut(&name) {
|
|
if topic.last_pub == Some(obs.id()) {
|
|
return Ok(());
|
|
}
|
|
let payload = String::from_utf8_lossy(&obs.payload).into_owned();
|
|
let msg = StrMsg { data: payload };
|
|
if let Some(publisher) = topic.publisher.as_mut() {
|
|
let _ = publisher.publish(&msg);
|
|
topic.last_pub = Some(obs.id());
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
/// The youngest observation for `tines_topic`, if any.
|
|
fn latest_obs(&self, tines_topic: &[u8], now: Tick) -> Option<Observation> {
|
|
let mut best: Option<Observation> = None;
|
|
self.tines
|
|
.store()
|
|
.for_each_topic(tines_topic, now, &mut |obs| match best {
|
|
Some(ref b) if obs.age_ms >= b.age_ms => {}
|
|
_ => best = Some(obs.clone()),
|
|
});
|
|
best
|
|
}
|
|
|
|
/// Publish every pending confidence event to [`RECEIPTS_TOPIC`].
|
|
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 _ = 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
|
|
}
|