From f7cced1c6960ad768703576c72008d86606bedbc Mon Sep 17 00:00:00 2001
From: Maximilian Schmidt <maximilian@schmidt.so>
Date: Fri, 3 Jul 2026 11:34:51 +0200
Subject: [PATCH 1/4] feat: recover low state source timestamps

---
 Cargo.lock                                 |   2 +
 crates/nodes/low_state_bridge/Cargo.toml   |   2 +
 crates/nodes/low_state_bridge/src/lib.rs   | 454 ++++++++++++++++++++-
 crates/ros-z/src/pubsub/publisher.rs       |  58 ++-
 crates/ros-z/tests/pubsub.rs               |  48 +++
 crates/ros2/src/sensor_msgs/joint_state.rs |  13 +
 crates/ros2/src/sensor_msgs/mod.rs         |   1 +
 7 files changed, 556 insertions(+), 22 deletions(-)
 create mode 100644 crates/ros2/src/sensor_msgs/joint_state.rs

diff --git a/Cargo.lock b/Cargo.lock
index e3499029a7..819fa8fdcb 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -6596,7 +6596,9 @@ dependencies = [
  "color-eyre",
  "kinematics",
  "ros-z",
+ "ros2",
  "tokio",
+ "tracing",
 ]
 
 [[package]]
diff --git a/crates/nodes/low_state_bridge/Cargo.toml b/crates/nodes/low_state_bridge/Cargo.toml
index f1103ac350..609672ebbe 100644
--- a/crates/nodes/low_state_bridge/Cargo.toml
+++ b/crates/nodes/low_state_bridge/Cargo.toml
@@ -11,4 +11,6 @@ cdr = { workspace = true }
 color-eyre = { workspace = true }
 kinematics = { workspace = true }
 ros-z = { workspace = true }
+ros2 = { workspace = true }
 tokio = { workspace = true, features = ["rt-multi-thread", "macros", "signal"] }
+tracing = { workspace = true }
diff --git a/crates/nodes/low_state_bridge/src/lib.rs b/crates/nodes/low_state_bridge/src/lib.rs
index fdce0aa655..28296affd8 100644
--- a/crates/nodes/low_state_bridge/src/lib.rs
+++ b/crates/nodes/low_state_bridge/src/lib.rs
@@ -1,3 +1,4 @@
+use std::collections::VecDeque;
 use std::sync::Arc;
 use std::{boxed::Box, future::Future, pin::Pin};
 
@@ -5,12 +6,244 @@ use color_eyre::{Result, eyre::Context as _};
 
 use booster::{ImuState, LowState, MotorState};
 use kinematics::joints::Joints;
-use ros_z::prelude::*;
+use ros_z::{prelude::*, time::Time};
+use ros2::sensor_msgs::joint_state::JointState;
 
 pub fn run_boxed(ctx: Arc<Context>) -> Pin<Box<dyn Future<Output = Result<()>> + Send>> {
     Box::pin(run(ctx))
 }
 
+const SAMPLE_BUFFER_LIMIT: usize = 128;
+
+#[derive(Clone, Debug, PartialEq, Eq)]
+struct SampleKey {
+    positions: Vec<u32>,
+    velocities: Vec<u32>,
+    efforts: Vec<u32>,
+}
+
+impl SampleKey {
+    fn from_low_state(low_state: &LowState) -> Self {
+        Self::from_f32_parts(
+            low_state
+                .motor_state_serial
+                .iter()
+                .map(|motor_state| motor_state.position),
+            low_state
+                .motor_state_serial
+                .iter()
+                .map(|motor_state| motor_state.velocity),
+            low_state
+                .motor_state_serial
+                .iter()
+                .map(|motor_state| motor_state.torque),
+        )
+    }
+
+    fn from_joint_state(joint_state: &JointState) -> Self {
+        Self::from_f32_parts(
+            joint_state.position.iter().map(|position| *position as f32),
+            joint_state.velocity.iter().map(|velocity| *velocity as f32),
+            joint_state.effort.iter().map(|effort| *effort as f32),
+        )
+    }
+
+    fn from_f32_parts(
+        positions: impl IntoIterator<Item = f32>,
+        velocities: impl IntoIterator<Item = f32>,
+        efforts: impl IntoIterator<Item = f32>,
+    ) -> Self {
+        Self {
+            positions: positions.into_iter().map(f32::to_bits).collect(),
+            velocities: velocities.into_iter().map(f32::to_bits).collect(),
+            efforts: efforts.into_iter().map(f32::to_bits).collect(),
+        }
+    }
+}
+
+#[derive(Debug)]
+struct PendingLowState {
+    sequence_number: u64,
+    key: SampleKey,
+    low_state: LowState,
+}
+
+#[derive(Debug)]
+struct JointStateStamp {
+    key: SampleKey,
+    source_time: Time,
+}
+
+#[derive(Debug)]
+struct MatchedLowState {
+    low_state: LowState,
+    source_time: Time,
+}
+
+#[derive(Debug, Default)]
+struct DropCounters {
+    low_state_buffer_overflows: u64,
+    joint_state_buffer_overflows: u64,
+    non_monotonic_low_states: u64,
+}
+
+#[derive(Debug, Default)]
+struct LowStateMatcher {
+    pending_low_states: VecDeque<PendingLowState>,
+    joint_state_stamps: VecDeque<JointStateStamp>,
+    next_sequence_number: u64,
+    last_published_source_time: Option<Time>,
+    drop_counters: DropCounters,
+}
+
+impl LowStateMatcher {
+    fn insert_low_state(&mut self, low_state: LowState) -> Option<MatchedLowState> {
+        let key = SampleKey::from_low_state(&low_state);
+        let sequence_number = self.reserve_sequence_number();
+
+        if let Some(index) = self
+            .joint_state_stamps
+            .iter()
+            .position(|joint_state| joint_state.key == key)
+        {
+            let joint_state = self
+                .joint_state_stamps
+                .remove(index)
+                .expect("joint state index came from position");
+            let low_state = PendingLowState {
+                sequence_number,
+                key,
+                low_state,
+            };
+
+            return self.accept_match(low_state, joint_state.source_time);
+        }
+
+        self.pending_low_states.push_back(PendingLowState {
+            sequence_number,
+            key,
+            low_state,
+        });
+
+        if self.pending_low_states.len() > SAMPLE_BUFFER_LIMIT {
+            self.pending_low_states.pop_front();
+            self.drop_counters.low_state_buffer_overflows += 1;
+            log_drop(
+                "low_state_buffer_overflow",
+                self.drop_counters.low_state_buffer_overflows,
+            );
+        }
+
+        None
+    }
+
+    fn insert_joint_state(&mut self, joint_state: JointState) -> Option<MatchedLowState> {
+        let joint_state = JointStateStamp {
+            key: SampleKey::from_joint_state(&joint_state),
+            source_time: joint_state.header.stamp.into(),
+        };
+
+        if let Some(index) = self
+            .pending_low_states
+            .iter()
+            .position(|low_state| low_state.key == joint_state.key)
+        {
+            let low_state = self
+                .pending_low_states
+                .remove(index)
+                .expect("low state index came from position");
+
+            return self.accept_match(low_state, joint_state.source_time);
+        }
+
+        self.joint_state_stamps.push_back(joint_state);
+
+        if self.joint_state_stamps.len() > SAMPLE_BUFFER_LIMIT {
+            self.joint_state_stamps.pop_front();
+            self.drop_counters.joint_state_buffer_overflows += 1;
+            log_drop(
+                "joint_state_buffer_overflow",
+                self.drop_counters.joint_state_buffer_overflows,
+            );
+        }
+
+        None
+    }
+
+    fn reserve_sequence_number(&mut self) -> u64 {
+        let sequence_number = self.next_sequence_number;
+        self.next_sequence_number += 1;
+        sequence_number
+    }
+
+    fn accept_match(
+        &mut self,
+        low_state: PendingLowState,
+        source_time: Time,
+    ) -> Option<MatchedLowState> {
+        if let Some(last_published_source_time) = self.last_published_source_time
+            && source_time < last_published_source_time
+        {
+            self.drop_counters.non_monotonic_low_states += 1;
+            tracing::debug!(
+                sequence_number = low_state.sequence_number,
+                matched_source_time = ?source_time,
+                ?last_published_source_time,
+                "dropping matched low_state because its source time would move backward"
+            );
+            log_drop(
+                "non_monotonic_low_state",
+                self.drop_counters.non_monotonic_low_states,
+            );
+            return None;
+        }
+
+        self.last_published_source_time = Some(source_time);
+        Some(MatchedLowState {
+            low_state: low_state.low_state,
+            source_time,
+        })
+    }
+}
+
+fn log_drop(kind: &'static str, count: u64) {
+    if count == 1 {
+        tracing::warn!(kind, count, "low_state_bridge dropped a buffered sample");
+    } else {
+        tracing::debug!(kind, count, "low_state_bridge dropped a buffered sample");
+    }
+}
+
+async fn publish_matched_low_state(
+    low_state_pub: &Publisher<LowState>,
+    imu_state_pub: &Publisher<ImuState>,
+    serial_motor_states_pub: &Publisher<Joints<MotorState>>,
+    parallel_motor_states_pub: &Publisher<Option<Joints<MotorState>>>,
+    matched_low_state: MatchedLowState,
+) -> Result<()> {
+    let source_time = matched_low_state.source_time;
+    let low_state = matched_low_state.low_state;
+
+    let imu_state = low_state.imu_state;
+    let serial_motor_states = low_state.serial_motor_states()?;
+    let parallel_motor_states = low_state.parallel_motor_states().ok();
+
+    low_state_pub
+        .publish_with_source_time(&low_state, source_time)
+        .await?;
+    imu_state_pub
+        .publish_with_source_time(&imu_state, source_time)
+        .await?;
+    serial_motor_states_pub
+        .publish_with_source_time(&serial_motor_states, source_time)
+        .await?;
+    parallel_motor_states_pub
+        .publish_with_source_time(&parallel_motor_states, source_time)
+        .await?;
+
+    Ok(())
+}
+
 async fn run(ctx: Arc<Context>) -> Result<()> {
     let node = ctx.create_node("low_state_bridge").build().await?;
 
@@ -20,6 +253,10 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
         .declare_subscriber("rt/low_state")
         .await
         .map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
+    let joint_state_sub = zenoh_session
+        .declare_subscriber("rt/joint_states")
+        .await
+        .map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
 
     let low_state_pub = node
         .publisher::<LowState>("inputs/low_state")
@@ -38,32 +275,215 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
         .build()
         .await?;
 
+    let mut matcher = LowStateMatcher::default();
+
     loop {
         tokio::select! {
             low_state = low_state_sub.recv_async() => {
                 let low_state = low_state.map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
 
                 let low_state: LowState = cdr::deserialize(&low_state.payload().to_bytes())
-                    .wrap_err("deserialization failed")?;
-
-                let imu_state = low_state.imu_state;
-                let serial_motor_states = low_state.serial_motor_states()?;
-                let parallel_motor_states = low_state.parallel_motor_states().ok();
+                    .wrap_err("low state deserialization failed")?;
 
-                low_state_pub
-                    .publish(&low_state)
-                    .await?;
-                imu_state_pub
-                    .publish(&imu_state)
-                    .await?;
-                serial_motor_states_pub
-                    .publish(&serial_motor_states)
-                    .await?;
-                parallel_motor_states_pub
-                    .publish(&parallel_motor_states)
+                if let Some(matched_low_state) = matcher.insert_low_state(low_state) {
+                    publish_matched_low_state(
+                        &low_state_pub,
+                        &imu_state_pub,
+                        &serial_motor_states_pub,
+                        &parallel_motor_states_pub,
+                        matched_low_state,
+                    )
                     .await?;
+                }
+            }
+            joint_state = joint_state_sub.recv_async() => {
+                let joint_state = joint_state.map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
+
+                let joint_state: JointState = cdr::deserialize(&joint_state.payload().to_bytes())
+                    .wrap_err("joint state deserialization failed")?;
 
+                if let Some(matched_low_state) = matcher.insert_joint_state(joint_state) {
+                    publish_matched_low_state(
+                        &low_state_pub,
+                        &imu_state_pub,
+                        &serial_motor_states_pub,
+                        &parallel_motor_states_pub,
+                        matched_low_state,
+                    )
+                    .await?;
+                }
             }
         }
     }
 }
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use booster::MotorState;
+    use ros2::{builtin_interfaces::time::Time as RosTime, std_msgs::header::Header};
+
+    fn low_state(values: &[(f32, f32, f32)]) -> LowState {
+        LowState {
+            imu_state: Default::default(),
+            motor_state_parallel: Vec::new(),
+            motor_state_serial: values
+                .iter()
+                .map(|(position, velocity, torque)| MotorState {
+                    position: *position,
+                    velocity: *velocity,
+                    torque: *torque,
+                    ..Default::default()
+                })
+                .collect(),
+        }
+    }
+
+    fn joint_state(nanos: i64, values: &[(f64, f64, f64)]) -> JointState {
+        let duration = std::time::Duration::from_nanos(nanos as u64);
+        JointState {
+            header: Header {
+                stamp: RosTime {
+                    sec: duration.as_secs() as i32,
+                    nanosec: duration.subsec_nanos(),
+                },
+                frame_id: String::new(),
+            },
+            name: Vec::new(),
+            position: values.iter().map(|(position, _, _)| *position).collect(),
+            velocity: values.iter().map(|(_, velocity, _)| *velocity).collect(),
+            effort: values.iter().map(|(_, _, effort)| *effort).collect(),
+        }
+    }
+
+    #[test]
+    fn low_state_waits_until_matching_joint_state_arrives() {
+        let mut matcher = LowStateMatcher::default();
+        let low_state = low_state(&[(1.0, 2.0, 3.0)]);
+
+        assert!(matcher.insert_low_state(low_state).is_none());
+
+        let matched = matcher.insert_joint_state(joint_state(10, &[(1.0, 2.0, 3.0)]));
+
+        assert_eq!(matched.unwrap().source_time, Time::from_nanos(10));
+        assert_eq!(matcher.pending_low_states.len(), 0);
+        assert_eq!(matcher.joint_state_stamps.len(), 0);
+    }
+
+    #[test]
+    fn joint_state_waits_until_matching_low_state_arrives() {
+        let mut matcher = LowStateMatcher::default();
+
+        assert!(
+            matcher
+                .insert_joint_state(joint_state(20, &[(1.0, 2.0, 3.0)]))
+                .is_none()
+        );
+
+        let matched = matcher.insert_low_state(low_state(&[(1.0, 2.0, 3.0)]));
+
+        assert_eq!(matched.unwrap().source_time, Time::from_nanos(20));
+        assert_eq!(matcher.pending_low_states.len(), 0);
+        assert_eq!(matcher.joint_state_stamps.len(), 0);
+    }
+
+    #[test]
+    fn joint_state_f64_values_match_low_state_f32_values() {
+        let mut matcher = LowStateMatcher::default();
+        let position = 0.1_f32;
+        let velocity = -0.2_f32;
+        let torque = 0.3_f32;
+
+        assert!(
+            matcher
+                .insert_joint_state(joint_state(
+                    30,
+                    &[(position as f64, velocity as f64, torque as f64)]
+                ))
+                .is_none()
+        );
+
+        let matched = matcher.insert_low_state(low_state(&[(position, velocity, torque)]));
+
+        assert_eq!(matched.unwrap().source_time, Time::from_nanos(30));
+    }
+
+    #[test]
+    fn duplicate_keys_consume_oldest_joint_state_first() {
+        let mut matcher = LowStateMatcher::default();
+
+        assert!(
+            matcher
+                .insert_joint_state(joint_state(40, &[(1.0, 2.0, 3.0)]))
+                .is_none()
+        );
+        assert!(
+            matcher
+                .insert_joint_state(joint_state(50, &[(1.0, 2.0, 3.0)]))
+                .is_none()
+        );
+
+        let first = matcher.insert_low_state(low_state(&[(1.0, 2.0, 3.0)]));
+        let second = matcher.insert_low_state(low_state(&[(1.0, 2.0, 3.0)]));
+
+        assert_eq!(first.unwrap().source_time, Time::from_nanos(40));
+        assert_eq!(second.unwrap().source_time, Time::from_nanos(50));
+    }
+
+    #[test]
+    fn low_state_buffer_overflow_drops_oldest_low_state() {
+        let mut matcher = LowStateMatcher::default();
+
+        for index in 0..=SAMPLE_BUFFER_LIMIT {
+            assert!(
+                matcher
+                    .insert_low_state(low_state(&[(index as f32, 0.0, 0.0)]))
+                    .is_none()
+            );
+        }
+
+        assert_eq!(matcher.pending_low_states.len(), SAMPLE_BUFFER_LIMIT);
+        assert_eq!(matcher.drop_counters.low_state_buffer_overflows, 1);
+        assert_eq!(
+            matcher.pending_low_states.front().unwrap().key,
+            SampleKey::from_f32_parts([1.0], [0.0], [0.0])
+        );
+    }
+
+    #[test]
+    fn joint_state_buffer_overflow_drops_oldest_joint_state() {
+        let mut matcher = LowStateMatcher::default();
+
+        for index in 0..=SAMPLE_BUFFER_LIMIT {
+            assert!(
+                matcher
+                    .insert_joint_state(joint_state(index as i64, &[(index as f64, 0.0, 0.0)]))
+                    .is_none()
+            );
+        }
+
+        assert_eq!(matcher.joint_state_stamps.len(), SAMPLE_BUFFER_LIMIT);
+        assert_eq!(matcher.drop_counters.joint_state_buffer_overflows, 1);
+        assert_eq!(
+            matcher.joint_state_stamps.front().unwrap().source_time,
+            Time::from_nanos(1)
+        );
+    }
+
+    #[test]
+    fn older_matched_timestamp_is_dropped_after_newer_publication() {
+        let mut matcher = LowStateMatcher::default();
+
+        let first = matcher.insert_joint_state(joint_state(100, &[(1.0, 0.0, 0.0)]));
+        assert!(first.is_none());
+        let first = matcher.insert_low_state(low_state(&[(1.0, 0.0, 0.0)]));
+        assert!(first.is_some());
+
+        let second = matcher.insert_joint_state(joint_state(90, &[(2.0, 0.0, 0.0)]));
+        assert!(second.is_none());
+        let second = matcher.insert_low_state(low_state(&[(2.0, 0.0, 0.0)]));
+
+        assert!(second.is_none());
+        assert_eq!(matcher.drop_counters.non_monotonic_low_states, 1);
+    }
+}
diff --git a/crates/ros-z/src/pubsub/publisher.rs b/crates/ros-z/src/pubsub/publisher.rs
index 5aa20053c9..04034b83ff 100644
--- a/crates/ros-z/src/pubsub/publisher.rs
+++ b/crates/ros-z/src/pubsub/publisher.rs
@@ -21,7 +21,7 @@ use crate::pubsub::metadata::PublicationId;
 use crate::pubsub::replay::{self, RetainedSample, TransientLocalCache};
 use crate::qos::QosProfile;
 use crate::shm::ShmConfig;
-use crate::time::Clock;
+use crate::time::{Clock, Time};
 use crate::topic_name;
 use ros_z_protocol::qos::{QosDurability, QosHistory, QosReliability};
 use ros_z_schema::SchemaBundle;
@@ -491,16 +491,20 @@ where
         PublicationId::new(self.endpoint_global_id, sequence_number)
     }
 
-    fn new_attachment_for_publication(&self, publication_id: PublicationId) -> Attachment {
+    fn new_attachment_for_publication_with_source_time(
+        &self,
+        publication_id: PublicationId,
+        source_time: Time,
+    ) -> Attachment {
         trace!(
             "[PUB] Creating attachment: sequence_number={}, endpoint_global_id={:02x?}",
             publication_id.sequence_number(),
             &publication_id.endpoint_global_id().as_bytes()[..4]
         );
-        Attachment::with_clock(
+        Attachment::with_source_time(
             publication_id.sequence_number(),
             publication_id.endpoint_global_id(),
-            &self.clock,
+            source_time,
         )
     }
 
@@ -516,12 +520,45 @@ where
         self.prepare().publish(message).await
     }
 
+    /// Serialize and publish `message` while using `source_time` in the ROS-Z attachment.
+    #[tracing::instrument(name = "publish_with_source_time", skip(self, message), fields(
+        topic = %self.entity.topic,
+        sequence_number = self.sequence_number.load(Ordering::Acquire),
+        endpoint_global_id = tracing::field::Empty,
+        payload_len = tracing::field::Empty,
+        used_shm = tracing::field::Empty
+    ))]
+    pub async fn publish_with_source_time(&self, message: &T, source_time: Time) -> Result<()> {
+        let publication_id = self.next_publication_id();
+        self.publish_with_reserved_id_and_source_time(message, publication_id, source_time)
+            .await
+    }
+
     async fn publish_with_reserved_id(
         &self,
         message: &T,
         publication_id: PublicationId,
     ) -> Result<()> {
         let (zbytes, attachment) = self.prepare_publish_payload(message, publication_id)?;
+        self.publish_payload(zbytes, attachment).await
+    }
+
+    async fn publish_with_reserved_id_and_source_time(
+        &self,
+        message: &T,
+        publication_id: PublicationId,
+        source_time: Time,
+    ) -> Result<()> {
+        let (zbytes, attachment) =
+            self.prepare_publish_payload_with_source_time(message, publication_id, source_time)?;
+        self.publish_payload(zbytes, attachment).await
+    }
+
+    async fn publish_payload(
+        &self,
+        zbytes: zenoh::bytes::ZBytes,
+        attachment: Attachment,
+    ) -> Result<()> {
         // Keep cache-before-publish semantics so replay queries can observe the retained
         // sample as soon as publish() returns, avoiding a race where a replay query arrives before
         // the sample is cached.
@@ -541,6 +578,16 @@ where
         &self,
         message: &T,
         publication_id: PublicationId,
+    ) -> Result<(zenoh::bytes::ZBytes, Attachment)> {
+        let source_time = self.clock.now();
+        self.prepare_publish_payload_with_source_time(message, publication_id, source_time)
+    }
+
+    fn prepare_publish_payload_with_source_time(
+        &self,
+        message: &T,
+        publication_id: PublicationId,
+        source_time: Time,
     ) -> Result<(zenoh::bytes::ZBytes, Attachment)> {
         tracing::Span::current().record(
             "endpoint_global_id",
@@ -591,7 +638,8 @@ where
         tracing::Span::current().record("payload_len", actual_size);
 
         let zbytes = zenoh::bytes::ZBytes::from(zbuf);
-        let attachment = self.new_attachment_for_publication(publication_id);
+        let attachment =
+            self.new_attachment_for_publication_with_source_time(publication_id, source_time);
 
         Ok((zbytes, attachment))
     }
diff --git a/crates/ros-z/tests/pubsub.rs b/crates/ros-z/tests/pubsub.rs
index 5943f04107..ea3c1b7250 100644
--- a/crates/ros-z/tests/pubsub.rs
+++ b/crates/ros-z/tests/pubsub.rs
@@ -572,6 +572,54 @@ async fn recv_with_metadata_includes_transport_and_source_timestamps() {
     );
 }
 
+#[tokio::test(flavor = "multi_thread", worker_threads = 2)]
+async fn publish_with_source_time_sets_metadata_source_time() {
+    let context = ContextBuilder::default()
+        .build()
+        .await
+        .expect("Failed to create context");
+    let node = context
+        .create_node("explicit_source_time_node")
+        .build()
+        .await
+        .expect("Failed to create node");
+
+    let publisher = node
+        .publisher::<TestMessage>("/explicit_source_time_topic")
+        .build()
+        .await
+        .unwrap();
+
+    let subscriber = node
+        .subscriber::<TestMessage>("/explicit_source_time_topic")
+        .build()
+        .await
+        .unwrap();
+
+    tokio::time::sleep(Duration::from_millis(100)).await;
+
+    let message = TestMessage {
+        data: vec![4, 5, 6],
+        counter: 42,
+    };
+    let source_time = Time::from_nanos(123_456_789);
+
+    publisher
+        .publish_with_source_time(&message, source_time)
+        .await
+        .unwrap();
+
+    let received = tokio::time::timeout(Duration::from_secs(1), subscriber.recv_with_metadata())
+        .await
+        .expect("receive should not time out")
+        .expect("receive should succeed");
+
+    assert_eq!(received.message, message);
+    assert_eq!(received.source_time, source_time);
+    assert_eq!(received.sequence_number, 0);
+    assert_ne!(received.source_global_id, ros_z::EndpointGlobalId::ZERO);
+}
+
 #[tokio::test(flavor = "multi_thread", worker_threads = 2)]
 async fn typed_subscriber_errors_when_sample_has_no_attachment() {
     let context = ContextBuilder::default()
diff --git a/crates/ros2/src/sensor_msgs/joint_state.rs b/crates/ros2/src/sensor_msgs/joint_state.rs
new file mode 100644
index 0000000000..a1cab725d5
--- /dev/null
+++ b/crates/ros2/src/sensor_msgs/joint_state.rs
@@ -0,0 +1,13 @@
+use serde::{Deserialize, Serialize};
+
+use crate::std_msgs::header::Header;
+
+#[repr(C)]
+#[derive(Clone, Debug, Default, Serialize, Deserialize)]
+pub struct JointState {
+    pub header: Header,
+    pub name: Vec<String>,
+    pub position: Vec<f64>,
+    pub velocity: Vec<f64>,
+    pub effort: Vec<f64>,
+}
diff --git a/crates/ros2/src/sensor_msgs/mod.rs b/crates/ros2/src/sensor_msgs/mod.rs
index 696dd06cfd..2aa4c897d0 100644
--- a/crates/ros2/src/sensor_msgs/mod.rs
+++ b/crates/ros2/src/sensor_msgs/mod.rs
@@ -1,6 +1,7 @@
 pub mod camera_info;
 pub mod image;
 pub mod imu;
+pub mod joint_state;
 pub mod magnetic_field;
 pub mod region_of_interest;
 pub mod time_reference;

From 0836897ba5166be1b52c25f4a2facac5b9cd55c3 Mon Sep 17 00:00:00 2001
From: Maximilian Schmidt <maximilian@schmidt.so>
Date: Fri, 3 Jul 2026 11:43:16 +0200
Subject: [PATCH 2/4] config: forward joint states over zenoh dds

---
 tools/k1-setup/zenoh-bridge-dds.json5 | 1 +
 1 file changed, 1 insertion(+)

diff --git a/tools/k1-setup/zenoh-bridge-dds.json5 b/tools/k1-setup/zenoh-bridge-dds.json5
index e1e877deaa..de1c7efdae 100644
--- a/tools/k1-setup/zenoh-bridge-dds.json5
+++ b/tools/k1-setup/zenoh-bridge-dds.json5
@@ -47,6 +47,7 @@
       ////
       allow: [
         "low_state",
+        "joint_states",
         "image_left_raw",
         "rectified_image",
         "kick_ball",

From 7725f38830346e21b20db375e9f9374185248579 Mon Sep 17 00:00:00 2001
From: Maximilian Schmidt <maximilian@schmidt.so>
Date: Sat, 4 Jul 2026 02:35:02 +0200
Subject: [PATCH 3/4] fix: address low state timestamp review

---
 crates/nodes/low_state_bridge/src/lib.rs   |  8 +++-
 crates/ros-z/src/pubsub/publisher.rs       |  6 +++
 crates/ros-z/tests/pubsub.rs               | 49 ++++++++++++++++++++++
 crates/ros2/src/sensor_msgs/joint_state.rs |  9 ++++
 docs/setup/upload.md                       |  5 +++
 5 files changed, 75 insertions(+), 2 deletions(-)

diff --git a/crates/nodes/low_state_bridge/src/lib.rs b/crates/nodes/low_state_bridge/src/lib.rs
index 28296affd8..d9f65a49d8 100644
--- a/crates/nodes/low_state_bridge/src/lib.rs
+++ b/crates/nodes/low_state_bridge/src/lib.rs
@@ -256,7 +256,9 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
     let joint_state_sub = zenoh_session
         .declare_subscriber("rt/joint_states")
         .await
-        .map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
+        .map_err(|error| {
+            color_eyre::eyre::eyre!("failed to declare subscriber for rt/joint_states: {error}")
+        })?;
 
     let low_state_pub = node
         .publisher::<LowState>("inputs/low_state")
@@ -297,7 +299,9 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
                 }
             }
             joint_state = joint_state_sub.recv_async() => {
-                let joint_state = joint_state.map_err(|error| color_eyre::eyre::eyre!("{error}"))?;
+                let joint_state = joint_state.map_err(|error| {
+                    color_eyre::eyre::eyre!("failed to receive sample from rt/joint_states: {error}")
+                })?;
 
                 let joint_state: JointState = cdr::deserialize(&joint_state.payload().to_bytes())
                     .wrap_err("joint state deserialization failed")?;
diff --git a/crates/ros-z/src/pubsub/publisher.rs b/crates/ros-z/src/pubsub/publisher.rs
index 04034b83ff..3e30f363da 100644
--- a/crates/ros-z/src/pubsub/publisher.rs
+++ b/crates/ros-z/src/pubsub/publisher.rs
@@ -181,6 +181,12 @@ where
             .publish_with_reserved_id(message, self.publication_id)
             .await
     }
+
+    pub async fn publish_with_source_time(self, message: &T, source_time: Time) -> Result<()> {
+        self.publisher
+            .publish_with_reserved_id_and_source_time(message, self.publication_id, source_time)
+            .await
+    }
 }
 
 impl<T, C: WireEncoder> std::fmt::Debug for Publisher<T, C> {
diff --git a/crates/ros-z/tests/pubsub.rs b/crates/ros-z/tests/pubsub.rs
index ea3c1b7250..76b8754e2e 100644
--- a/crates/ros-z/tests/pubsub.rs
+++ b/crates/ros-z/tests/pubsub.rs
@@ -620,6 +620,55 @@ async fn publish_with_source_time_sets_metadata_source_time() {
     assert_ne!(received.source_global_id, ros_z::EndpointGlobalId::ZERO);
 }
 
+#[tokio::test(flavor = "multi_thread", worker_threads = 2)]
+async fn prepared_publish_with_source_time_sets_metadata_source_time() {
+    let context = ContextBuilder::default()
+        .build()
+        .await
+        .expect("Failed to create context");
+    let node = context
+        .create_node("prepared_explicit_source_time_node")
+        .build()
+        .await
+        .expect("Failed to create node");
+
+    let publisher = node
+        .publisher::<TestMessage>("/prepared_explicit_source_time_topic")
+        .build()
+        .await
+        .unwrap();
+
+    let subscriber = node
+        .subscriber::<TestMessage>("/prepared_explicit_source_time_topic")
+        .build()
+        .await
+        .unwrap();
+
+    tokio::time::sleep(Duration::from_millis(100)).await;
+
+    let message = TestMessage {
+        data: vec![6, 5, 4],
+        counter: 43,
+    };
+    let source_time = Time::from_nanos(987_654_321);
+
+    publisher
+        .prepare()
+        .publish_with_source_time(&message, source_time)
+        .await
+        .unwrap();
+
+    let received = tokio::time::timeout(Duration::from_secs(1), subscriber.recv_with_metadata())
+        .await
+        .expect("receive should not time out")
+        .expect("receive should succeed");
+
+    assert_eq!(received.message, message);
+    assert_eq!(received.source_time, source_time);
+    assert_eq!(received.sequence_number, 0);
+    assert_ne!(received.source_global_id, ros_z::EndpointGlobalId::ZERO);
+}
+
 #[tokio::test(flavor = "multi_thread", worker_threads = 2)]
 async fn typed_subscriber_errors_when_sample_has_no_attachment() {
     let context = ContextBuilder::default()
diff --git a/crates/ros2/src/sensor_msgs/joint_state.rs b/crates/ros2/src/sensor_msgs/joint_state.rs
index a1cab725d5..dcf9c5fc83 100644
--- a/crates/ros2/src/sensor_msgs/joint_state.rs
+++ b/crates/ros2/src/sensor_msgs/joint_state.rs
@@ -4,10 +4,19 @@ use crate::std_msgs::header::Header;
 
 #[repr(C)]
 #[derive(Clone, Debug, Default, Serialize, Deserialize)]
+/// State of a set of joints at `header.stamp`.
+///
+/// Values in `name`, `position`, `velocity`, and `effort` correspond by index.
+/// Numeric arrays may be empty when that state is not available.
 pub struct JointState {
+    /// Coordinate frame and sample timestamp.
     pub header: Header,
+    /// Joint names in the same order as the numeric state arrays.
     pub name: Vec<String>,
+    /// Joint positions in radians or meters.
     pub position: Vec<f64>,
+    /// Joint velocities in radians per second or meters per second.
     pub velocity: Vec<f64>,
+    /// Joint efforts in Newton meters or Newtons.
     pub effort: Vec<f64>,
 }
diff --git a/docs/setup/upload.md b/docs/setup/upload.md
index 078a93843c..db620b325a 100644
--- a/docs/setup/upload.md
+++ b/docs/setup/upload.md
@@ -13,6 +13,11 @@ pepsi upload <NAO>
 Pepsi takes care of downloading and installing the SDK, calling `cargo` to trigger compilation, and uploading the software to the robot.
 When successful, you are presented with a green tick in your shell, and the robot is showing rotating rainbow eyes.
 
+## Runtime Topics
+
+The `low_state_bridge` process publishes low-state-derived inputs only after it can match `rt/low_state` samples with timestamped `rt/joint_states` samples.
+If these inputs stop updating after deployment, verify that both topics are produced and that `joint_states` is allowed through the DDS bridge configuration.
+
 !!! tip
 
     Continue reading [here](../workflow/overview.md) about our workflow and how to contribute to the project.

From a87fc4e3b645367314937807f8f8421e5510ada2 Mon Sep 17 00:00:00 2001
From: Maximilian Schmidt <maximilian@schmidt.so>
Date: Sat, 4 Jul 2026 12:42:19 +0200
Subject: [PATCH 4/4] fix: remove misplaced runtime topic docs

---
 docs/setup/upload.md | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/docs/setup/upload.md b/docs/setup/upload.md
index db620b325a..078a93843c 100644
--- a/docs/setup/upload.md
+++ b/docs/setup/upload.md
@@ -13,11 +13,6 @@ pepsi upload <NAO>
 Pepsi takes care of downloading and installing the SDK, calling `cargo` to trigger compilation, and uploading the software to the robot.
 When successful, you are presented with a green tick in your shell, and the robot is showing rotating rainbow eyes.
 
-## Runtime Topics
-
-The `low_state_bridge` process publishes low-state-derived inputs only after it can match `rt/low_state` samples with timestamped `rt/joint_states` samples.
-If these inputs stop updating after deployment, verify that both topics are produced and that `joint_states` is allowed through the DDS bridge configuration.
-
 !!! tip
 
     Continue reading [here](../workflow/overview.md) about our workflow and how to contribute to the project.