Task 9: tines-ros2 — pure topic_name escaping (default, tested) + optional rclrs TinesNode (feature ros2)
Adds the ninth fleet-deployment crate. Default build is ROS2-free: topic_name hex-escapes arbitrary tines topics into valid ROS2 names under /tines/ (deterministic, injective) and wires Node<LogStore, BtTransport>. The rclrs-backed self-contained TinesNode lives under #[cfg(feature="ros2")] with an off-by-default ros2 feature, so the crate builds/tests without a ROS2 toolchain. Adds scripts/check-ros2.sh (needs a ROS2 toolchain; not run in CI), registers the crate in workspace members, and passes sh scripts/ci.sh.
This commit is contained in:
@@ -0,0 +1,324 @@
|
||||
//! 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.
|
||||
let managed: Vec<String> = self.managed.keys().cloned().collect();
|
||||
for name in managed {
|
||||
if let Some(topic) = self.managed.get_mut(&name) {
|
||||
self.publish_one(&topic.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> {
|
||||
let topic = self
|
||||
.managed
|
||||
.get_mut(&name)
|
||||
.expect("managed topic still present");
|
||||
if let (Some(publisher), Some(obs)) =
|
||||
(topic.publisher.as_mut(), self.latest_obs(tines_topic, now))
|
||||
{
|
||||
if topic.last_pub == Some(obs.id()) {
|
||||
return Ok(());
|
||||
}
|
||||
let payload = String::from_utf8_lossy(&obs.payload).into_owned();
|
||||
let msg = StrMsg { data: payload };
|
||||
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
|
||||
}
|
||||
Reference in New Issue
Block a user