Fix state freshness and make snapshots atomic

piper_cpp/include/piper_cpp/interface/piper_interface_v2.h

@@ -74,9 +74,10 @@ class PiperInterfaceV2
     /// @return true if the port opened (or was already open); false on initialisation failure.
     bool connectPort(bool can_init = false, bool piper_init = true, bool start_threads = true);
 
-    /// Stop the background read thread (if running) and close the CAN port. Wakes the read
-    /// thread out of its blocking receive and joins it, so this returns promptly even on a
-    /// silent bus. `timeout` is currently unused and kept for API compatibility. Idempotent.
+    /// Stop the background read thread (if running), close the CAN port and clear every cached
+    /// state, so a later session cannot read this one's feedback. Wakes the read thread out of
+    /// its blocking receive and joins it, so this returns promptly even on a silent bus.
+    /// `timeout` is currently unused and kept for API compatibility. Idempotent.
     void disconnectPort(std::chrono::milliseconds timeout = std::chrono::milliseconds{100});
 
     /// Fire the one-shot startup queries: per-motor angle/speed limits, per-motor acceleration
@@ -93,11 +94,13 @@ class PiperInterfaceV2
     //   - `value`        : the payload itself
     //   - `timestamp`    : Unix-epoch timestamp (in seconds, double) of the last update
     //   - `hz`           : measured update rate based on inter-arrival time
-    //   - `is_valid`     : sticky -- true once at least one frame has been received
-    //   - `is_healthy`   : true if a frame has arrived in the last ~1 second
+    //   - `is_valid`     : true once every part of the message has been received this session
+    //   - `is_healthy`   : for periodic feedback, true while every part arrived within the last
+    //                      second; query responses and command echoes stay healthy once received
     //
-    // `is_valid` is only false during the brief window between `connectPort` returning and the
-    // first matching CAN frame arriving. Use `is_healthy` to detect a stalled or dropped feed.
+    // All five fields describe the same update. `is_valid` is false between `connectPort`
+    // returning and the first matching CAN frame arriving, and again after `disconnectPort`,
+    // which clears every cached state. Use `is_healthy` to detect a stalled or dropped feed.
 
     /// Latest arm status: control mode, motion status, error codes, teaching state, etc.
     /// Updates at ~200 Hz when the arm is connected. Drives `getArmHealth()`'s status field.
@@ -130,7 +133,8 @@ class PiperInterfaceV2
 
     /// End-effector velocity/acceleration limits the arm currently has set (0.001 m/s,
     /// 0.001 rad/s, etc.). Response message; populated only after a periodic-feedback request
-    /// is enabled or after `sendArmParamEnquiryAndConfig(EndVelAcc)` is called.
+    /// is enabled or after `sendArmParamEnquiryAndConfig(EndVelAcc)` is called. Because it can
+    /// be streamed, it keeps the 1 s health timeout; after a one-shot enquiry rely on `is_valid`.
     StateSnapshot<ArmMsgCurrentEndVelAccParam> getArmCurrentEndVelAcc() const
     {
         return StateSnapshot(arm_current_end_vel_acc_);
@@ -673,6 +677,8 @@ class PiperInterfaceV2
     ArmMsgMotionCtrl_2 ctrl_151_tx_;
     bool sendCommandedMotionCtrlLocked();
 
+    void resetStates();
+
     void updateFeedbackFK();
 
     void updateCtrlFK();

piper_cpp/include/piper_cpp/interface/timed_freq_state.h

@@ -5,18 +5,24 @@
 #include <mutex>
 #include <numeric>
 #include <sstream>
+#include <utility>
+#include <vector>
 
 namespace piper_cpp
 {
 
+template <typename T> struct StateSnapshot;
+
+/// Pass to `TimedFreqState::setHealthTimeout()` for one-shot responses, which stay healthy
+/// once received.
+inline constexpr std::chrono::steady_clock::duration kNoHealthTimeout = std::chrono::steady_clock::duration::max();
+
+/// A value updated from the CAN read thread, with per-part arrival tracking so callers can tell
+/// a live stream from a stale one. Multipart messages register one caller per part.
 template <typename T> class TimedFreqState
 {
-
     using clock = std::chrono::steady_clock;
 
-    // if a set() call is not received within this time, the data stream is considered unhealthy.
-    const uint HEALTH_TIMEOUT_MS = 1000;
-
 public:
     TimedFreqState()
     {
@@ -26,62 +32,63 @@ template <typename T> class TimedFreqState
     void setNumCallers(size_t num_callers)
     {
         std::lock_guard<std::mutex> lk(mutex_);
-        last_timestamp_.resize(num_callers, 0.0);
+        last_received_.resize(num_callers);
         initialized_.resize(num_callers, false);
         hz_.resize(num_callers, 0.0);
     }
 
     size_t getNumCallers() const
     {
         std::lock_guard<std::mutex> lk(mutex_);
-        return static_cast<size_t>(last_timestamp_.size());
+        return last_received_.size();
+    }
+
+    /// How long after the last `set()` of any part the state stops being healthy. Defaults to
+    /// one second, which suits the periodic feedback streams.
+    void setHealthTimeout(clock::duration timeout)
+    {
+        std::lock_guard<std::mutex> lk(mutex_);
+        health_timeout_ = timeout;
     }
 
     /// Atomically replace the contained value, and update timestamp/hz.
     void set(const T& newValue, double timestamp, int caller_id = 0)
     {
         std::lock_guard<std::mutex> lk(mutex_);
-        double now_sec =
-            std::chrono::duration_cast<std::chrono::duration<double>>(clock::now().time_since_epoch()).count();
+        const auto now = clock::now();
 
         // compute hz only if we've been initialized once before
         if (initialized_.at(caller_id))
         {
-            double dt = now_sec - last_timestamp_.at(caller_id);
+            const double dt = std::chrono::duration<double>(now - last_received_.at(caller_id)).count();
             hz_.at(caller_id) = (dt > 0.0) ? (1.0 / dt) : 0.0;
         }
         value_ = newValue;
-        timestamp_ = timestamp; // Use provided timestamp, not now_sec
-        last_timestamp_.at(caller_id) = now_sec;
+        timestamp_ = timestamp; // Use provided timestamp, not now
+        last_received_.at(caller_id) = now;
         initialized_.at(caller_id) = true;
     }
 
+    /// Forget the value and every part's arrival, as at construction.
+    void reset()
+    {
+        std::lock_guard<std::mutex> lk(mutex_);
+        value_ = T{};
+        timestamp_ = 0.0;
+        std::fill(initialized_.begin(), initialized_.end(), false);
+        std::fill(hz_.begin(), hz_.end(), 0.0);
+    }
+
     bool isValid() const
     {
         std::lock_guard<std::mutex> lk(mutex_);
-        return initialized_.size() > 0 &&
-               std::all_of(initialized_.begin(), initialized_.end(), [](bool init) { return init; });
+        return isValidLocked();
     }
 
     bool isHealthy() const
     {
-        if (!isValid())
-        {
-            return false;
-        }
-        {
-            std::lock_guard<std::mutex> lk(mutex_);
-            double now_sec =
-                std::chrono::duration_cast<std::chrono::duration<double>>(clock::now().time_since_epoch()).count();
-            for (size_t i = 0; i < last_timestamp_.size(); ++i)
-            {
-                if (last_timestamp_.at(i) + HEALTH_TIMEOUT_MS < now_sec)
-                {
-                    return false;
-                }
-            }
-            return true;
-        }
+        std::lock_guard<std::mutex> lk(mutex_);
+        return isHealthyLocked(clock::now());
     }
 
     /// Atomically read out the payload
@@ -100,15 +107,39 @@ template <typename T> class TimedFreqState
     double getHz() const
     {
         std::lock_guard<std::mutex> lk(mutex_);
-        return std::accumulate(hz_.begin(), hz_.end(), 0.0) / hz_.size();
+        return getHzLocked();
     }
 
+    /// Value and metadata taken under one lock, so they describe the same update.
+    StateSnapshot<T> snapshot() const;
+
 private:
+    bool isValidLocked() const
+    {
+        return initialized_.size() > 0 &&
+               std::all_of(initialized_.begin(), initialized_.end(), [](bool init) { return init; });
+    }
+
+    bool isHealthyLocked(clock::time_point now) const
+    {
+        if (!isValidLocked())
+            return false;
+        for (const auto& received : last_received_)
+        {
+            if (now - received > health_timeout_)
+                return false;
+        }
+        return true;
+    }
+
+    double getHzLocked() const { return std::accumulate(hz_.begin(), hz_.end(), 0.0) / hz_.size(); }
+
     mutable std::mutex mutex_;
     T value_{};
     double timestamp_{0.0};
+    clock::duration health_timeout_{std::chrono::seconds(1)};
     std::vector<double> hz_; // Instantaneous rate
-    std::vector<double> last_timestamp_{};
+    std::vector<clock::time_point> last_received_;
     std::vector<bool> initialized_;
 };
 
@@ -120,9 +151,10 @@ template <typename T> struct StateSnapshot
     bool is_valid;
     bool is_healthy;
 
-    StateSnapshot(const TimedFreqState<T>& state)
-        : value(state.getValue()), timestamp(state.getTimestamp()), hz(state.getHz()), is_valid(state.isValid()),
-          is_healthy(state.isHealthy())
+    StateSnapshot(const TimedFreqState<T>& state) : StateSnapshot(state.snapshot()) {}
+
+    StateSnapshot(T value_, double timestamp_, double hz_, bool is_valid_, bool is_healthy_)
+        : value(std::move(value_)), timestamp(timestamp_), hz(hz_), is_valid(is_valid_), is_healthy(is_healthy_)
     {
     }
 
@@ -135,4 +167,11 @@ template <typename T> struct StateSnapshot
     }
 };
 
+template <typename T> StateSnapshot<T> TimedFreqState<T>::snapshot() const
+{
+    std::lock_guard<std::mutex> lk(mutex_);
+    const auto now = clock::now();
+    return StateSnapshot<T>(value_, timestamp_, getHzLocked(), isValidLocked(), isHealthyLocked(now));
+}
+
 } // namespace piper_cpp

piper_cpp/src/interface/piper_interface_v2.cpp

@@ -40,6 +40,19 @@ PiperInterfaceV2::PiperInterfaceV2(
     arm_motion_ctrl_code_151_.setNumCallers(1);
     arm_instruction_response_.setNumCallers(1);
 
+    // Query responses and command echoes arrive once, not periodically, so their age says
+    // nothing about the link; only the periodic feedback streams keep the default timeout.
+    arm_crash_protection_rating_fb_.setHealthTimeout(kNoHealthTimeout);
+    arm_gripper_teaching_pendant_fb_.setHealthTimeout(kNoHealthTimeout);
+    arm_current_motor_angle_limit_max_spd_.setHealthTimeout(kNoHealthTimeout);
+    arm_current_motor_max_acc_limit_.setHealthTimeout(kNoHealthTimeout);
+    arm_all_motor_angle_limit_max_spd_.setHealthTimeout(kNoHealthTimeout);
+    arm_all_motor_max_acc_limit_.setHealthTimeout(kNoHealthTimeout);
+    arm_joint_ctrl_msgs_.setHealthTimeout(kNoHealthTimeout);
+    arm_gripper_ctrl_.setHealthTimeout(kNoHealthTimeout);
+    arm_motion_ctrl_code_151_.setHealthTimeout(kNoHealthTimeout);
+    arm_instruction_response_.setHealthTimeout(kNoHealthTimeout);
+
     /// TODO: initialise other members as in python
 }
 
@@ -116,9 +129,35 @@ void PiperInterfaceV2::disconnectPort(std::chrono::milliseconds timeout)
         read_thread_.join();
     }
     arm_can_->close();
+    resetStates();
     connected_.store(false, std::memory_order_relaxed);
 }
 
+void PiperInterfaceV2::resetStates()
+{
+    // Called with the read thread stopped, so nothing races these.
+    arm_status_.reset();
+    arm_end_pose_.reset();
+    arm_joint_states_.reset();
+    arm_gripper_msgs_.reset();
+    arm_high_spd_fb_.reset();
+    arm_low_spd_fb_.reset();
+    arm_current_end_vel_acc_.reset();
+    arm_crash_protection_rating_fb_.reset();
+    arm_gripper_teaching_pendant_fb_.reset();
+    arm_current_motor_angle_limit_max_spd_.reset();
+    arm_current_motor_max_acc_limit_.reset();
+    arm_all_motor_angle_limit_max_spd_.reset();
+    arm_all_motor_max_acc_limit_.reset();
+    arm_joint_ctrl_msgs_.reset();
+    arm_gripper_ctrl_.reset();
+    arm_motion_ctrl_code_151_.reset();
+    arm_instruction_response_.reset();
+    arm_health_.reset();
+    std::lock_guard<std::mutex> lk(firmware_data_mutex_);
+    firmware_data_.clear();
+}
+
 void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double timestamp)
 {
     // Parse the CAN frame using the parser

piper_cpp/test/CMakeLists.txt

@@ -13,3 +13,4 @@ endfunction()
 add_piper_test(std_can_interface_test)
 add_piper_test(piper_interface_v2_test)
 add_piper_test(piper_parser_v2_test)
+add_piper_test(timed_freq_state_test)

piper_cpp/test/piper_interface_v2_test.cpp

@@ -186,3 +186,19 @@ TEST(PiperInterfaceV2, AllMotorAccLimitsValidOnlyAfterEveryMotorReplies)
         EXPECT_EQ(all.motor[i].max_joint_acc, i + 1);
     }
 }
+
+TEST(PiperInterfaceV2, DisconnectClearsCachedState)
+{
+    auto bus = makeBus();
+    ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true));
+    const auto deadline = steady_clock::now() + 5s;
+    sendFrame(bus.peer, jointFeedbackFrame(0x2A5, 1, 2), deadline);
+    sendFrame(bus.peer, jointFeedbackFrame(0x2A6, 3, 4), deadline);
+    sendFrame(bus.peer, jointFeedbackFrame(0x2A7, 5, 6), deadline);
+    ASSERT_TRUE(waitFor([&] { return bus.piper->getArmJointStates().is_valid; }));
+
+    bus.piper->disconnectPort();
+    const auto js = bus.piper->getArmJointStates();
+    EXPECT_FALSE(js.is_valid) << "feedback from a closed session must not look current";
+    EXPECT_EQ(js.value.joint_6, 0);
+}