fix: harden SRC1100 free navigation handling

cmvr-es/devices/agv/src1100/include/src1100_agv.h

@@ -2,6 +2,7 @@
 #define CMVR_ES_SRC1100_AGV_H
 
 #include <atomic>
+#include <chrono>
 #include <condition_variable>
 #include <cstdint>
 #include <deque>
@@ -77,6 +78,25 @@ private:
         int push{19301};
     };
 
+    struct PoseTaskStatus {
+        bool found{false};
+        int state{0};
+        int type{0};
+        double progress{0.0};
+        std::string detail;
+    };
+
+    struct PoseTaskContext {
+        std::string task_id;
+        math::Pose2d target{};
+        double reach_distance{0.0};
+        double reach_angle{0.0};
+        std::uint64_t navigation_generation{0};
+        std::uint64_t controller_fault_sequence_at_start{0};
+        std::uint64_t control_attempt_sequence_at_start{0};
+        std::uint64_t controller_fault_channel_epoch_at_start{0};
+    };
+
     AgvResult connect_();
     AgvResult disconnect_();
     AgvResult connectSocket_(int& sock, int port);
@@ -86,13 +106,37 @@ private:
 
     AgvResult acquireControl_() const;
     AgvResult confirmPoseNavigationStarted_(
+        const PoseTaskContext& context) const;
+    AgvResult queryPoseTaskStatus_(
         const std::string& task_id,
-        std::uint64_t navigation_generation) const;
+        PoseTaskStatus& status) const;
+    bool poseTargetReached_(
+        const PoseTaskContext& context,
+        std::string& detail) const;
+    std::string cachedControllerFaultDetail_(
+        std::uint64_t after_sequence = 0,
+        int wait_ms = 0,
+        std::uint64_t* associated_control_attempt = nullptr) const;
+    int controllerFaultCaptureGraceMs_() const;
+    int controllerFaultStateMaxAgeMs_() const;
+    std::string freeNavigationFaultStateUnavailableDetail_() const;
+    void rememberPoseTask_(const PoseTaskContext& context) const;
+    void advancePoseTaskGeneration_(
+        std::uint64_t navigation_generation,
+        std::uint64_t control_attempt_sequence) const;
+    void advancePoseTaskControlAttempt_(
+        std::uint64_t control_attempt_sequence) const;
+    void clearPoseTask_(std::uint64_t navigation_generation) const;
+    bool currentPoseTask_(PoseTaskContext& context) const;
     AgvResult sendControlledCommand_(int sock,
                                      std::uint16_t command,
                                      const Json::Value& payload,
                                      Json::Value* response,
-                                     std::uint64_t* accepted_navigation_generation = nullptr) const;
+                                     std::uint64_t* accepted_navigation_generation = nullptr,
+                                     std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
+                                     std::uint64_t* control_attempt_sequence = nullptr,
+                                     PoseTaskContext* pose_context_to_publish = nullptr,
+                                     bool reject_if_active_controller_fault = false) const;
     AgvResult sendCommand_(int sock,
                            std::uint16_t command,
                            const Json::Value& payload,
@@ -106,6 +150,7 @@ private:
     void startPushThread_();
     void stopPushThread_();
     void pushLoop_();
+    void invalidateControllerFaultState_();
     AgvRuntimeState queryRuntimeState_() const;
     void updateCachedRuntimeState_(const Json::Value& payload);
     void startMapUpdateThread_();
@@ -168,6 +213,10 @@ private:
     mutable std::mutex control_sequence_mutex_;
     mutable std::atomic<std::uint64_t> navigation_generation_{0};
     mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
+    mutable std::atomic<std::uint64_t> control_attempt_sequence_{0};
+    mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
+    mutable std::mutex pose_task_mutex_;
+    mutable PoseTaskContext pose_task_context_;
     mutable int sock_status_{-1};
     mutable int sock_control_{-1};
     mutable int sock_navigation_{-1};
@@ -179,8 +228,16 @@ private:
     std::atomic<bool> push_running_{false};
     std::thread push_thread_;
     mutable std::mutex runtime_state_mutex_;
+    mutable std::condition_variable runtime_state_cv_;
     AgvRuntimeState cached_runtime_state_;
     bool cached_runtime_state_valid_{false};
+    std::uint64_t controller_fault_sequence_{0};
+    bool controller_fault_state_observed_{false};
+    std::chrono::steady_clock::time_point controller_fault_state_observed_at_{};
+    std::string active_controller_fault_detail_;
+    double last_controller_fault_timestamp_{0.0};
+    std::string last_controller_fault_detail_;
+    std::uint64_t last_controller_fault_control_attempt_{0};
 
     mutable std::atomic<bool> map_update_running_{false};
     mutable std::thread map_update_thread_;