Feat/feishu history

config/config.yaml

@@ -6,6 +6,7 @@ capturer:
   show_loss_framerate: false
   show_loss_framerate_interval: 500
   reconnect_wait_interval: 100
+  enable_trigger_sync: true
   # hikcamera or local_video
   source: "hikcamera"
   hikcamera:
@@ -21,7 +22,7 @@ capturer:
     invert_image: false
     software_sync: false
     trigger_mode: false
-    fixed_framerate: false
+    fixed_framerate: true
   local_video:
     # 替换为你具体的路径
     location: "/workspaces/alliance/test_videos/outpost.mp4"

src/component.cpp

@@ -1,6 +1,7 @@
 #include "kernel/feishu.hpp"
 #include "module/debug/action_throttler.hpp"
 #include "module/debug/framerate.hpp"
+#include "utility/clock.hpp"
 #include "utility/rclcpp/node.hpp"
 #include "utility/rclcpp/visual/transform.hpp"
 #include "utility/shared/context.hpp"
@@ -15,13 +16,17 @@ namespace rmcs {
 
 using namespace rmcs::util;
 using namespace kernel;
+using Clock = util::Clock;
 
 class AutoAimComponent final : public rmcs_executor::Component {
 public:
     explicit AutoAimComponent() noexcept
         : rclcpp { get_component_name() } {
 
+        register_input("/predefined/timestamp", predefined_timestamp_);
         register_input("/tf", rmcs_tf);
+        register_input("/camera/trigger/seq", camera_trigger_seq_);
+        register_input("/camera/trigger/timestamp", camera_trigger_timestamp_);
 
         register_output("/gimbal/auto_aim/auto_aim_enabled", gimbal_takeover, false);
         register_output(
@@ -41,6 +46,9 @@ class AutoAimComponent final : public rmcs_executor::Component {
 
         action_throttler.register_action("tf_not_ready");
         action_throttler.register_action("commit_control_state_failed");
+        action_throttler.register_action("commit_camera_trigger_failed");
+        action_throttler.register_action("camera_trigger_gap_detected");
+        action_throttler.register_action("camera_trigger_not_ready");
     }
 
     auto update() -> void override {
@@ -56,18 +64,24 @@ class AutoAimComponent final : public rmcs_executor::Component {
 private:
     static constexpr auto auto_aim_state_timeout { std::chrono::milliseconds { 100 } };
 
+    InputInterface<Clock::time_point> predefined_timestamp_;
     InputInterface<rmcs_description::Tf> rmcs_tf;
 
+    InputInterface<std::uint64_t> camera_trigger_seq_;
+    InputInterface<Clock::time_point> camera_trigger_timestamp_;
+
     double current_gimbal_yaw { std::numeric_limits<double>::quiet_NaN() };
     double current_gimbal_pitch { std::numeric_limits<double>::quiet_NaN() };
 
     RclcppNode rclcpp;
     std::unique_ptr<visual::Transform> visual_odom_to_camera;
 
     Feishu<RuntimeRole::Control> feishu;
+    Channel<CameraTriggerEvent> camera_trigger_channel;
     ControlState control_state;
     AutoAimState auto_aim_state;
     bool auto_aim_state_received_ { false };
+    std::uint64_t last_committed_camera_trigger_seq_ { 0 };
 
     OutputInterface<bool> gimbal_takeover;
     OutputInterface<bool> shoot_permitted;
@@ -138,13 +152,56 @@ class AutoAimComponent final : public rmcs_executor::Component {
         if (!success) {
             action_throttler.dispatch("commit_control_state_failed",
                 [&] { rclcpp.info("commit control state failed!"); });
+            return;
+        }
+
+        action_throttler.reset("commit_control_state_failed");
+
+        if (!camera_trigger_seq_.ready() || !camera_trigger_timestamp_.ready()) [[unlikely]] {
+            action_throttler.dispatch("camera_trigger_not_ready", [&] {
+                rclcpp.warn("camera trigger input is not ready, skip publishing trigger event");
+            });
+            return;
+        }
+
+        action_throttler.reset("camera_trigger_not_ready");
+        const auto trigger_seq       = *camera_trigger_seq_;
+        const auto trigger_timestamp = *camera_trigger_timestamp_;
+        publish_camera_trigger_event(trigger_seq, trigger_timestamp);
+    }
+
+    auto publish_camera_trigger_event(
+        std::uint64_t trigger_seq, Clock::time_point trigger_timestamp) -> void {
+        if (trigger_seq == 0 || trigger_seq == last_committed_camera_trigger_seq_) {
+            return;
+        }
+
+        if (last_committed_camera_trigger_seq_ != 0
+            && trigger_seq > last_committed_camera_trigger_seq_ + 1) {
+            action_throttler.dispatch("camera_trigger_gap_detected", [&] {
+                rclcpp.warn("Camera trigger gap detected: last={}, current={}",
+                    last_committed_camera_trigger_seq_, trigger_seq);
+            });
         } else {
-            action_throttler.reset("commit_control_state_failed");
+            action_throttler.reset("camera_trigger_gap_detected");
         }
+
+        auto success = camera_trigger_channel.commit(CameraTriggerEvent {
+            .seq       = trigger_seq,
+            .timestamp = trigger_timestamp,
+        });
+        if (!success) {
+            action_throttler.dispatch("commit_camera_trigger_failed",
+                [&] { rclcpp.info("commit camera trigger event failed!"); });
+            return;
+        }
+
+        last_committed_camera_trigger_seq_ = trigger_seq;
+        action_throttler.reset("commit_camera_trigger_failed");
     }
 
     auto update_control_state() -> void {
-        control_state.timestamp = Clock::now();
+        control_state.timestamp = *predefined_timestamp_;
 
         auto odom_to_camera_transform =
             fast_tf::lookup_transform<rmcs_description::OdomImu, rmcs_description::CameraLink>(

src/kernel/capturer.cpp

@@ -1,4 +1,5 @@
 #include "capturer.hpp"
+#include "kernel/feishu.hpp"
 #include "module/capturer/common.hpp"
 #include "module/capturer/hikcamera.hpp"
 #include "module/capturer/local_video.hpp"
@@ -8,6 +9,7 @@
 #include "utility/thread/spsc_queue.hpp"
 #include "utility/times_limit.hpp"
 
+#include <optional>
 #include <rclcpp/utilities.hpp>
 #include <thread>
 
@@ -23,9 +25,13 @@ struct Capturer::Impl {
     FramerateCounter loss_image_framerate {};
 
     std::chrono::milliseconds reconnect_wait_interval { 500 };
+    std::chrono::milliseconds trigger_sync_max_age { 50 };
 
     util::spsc_queue<Image*, 10> capture_queue;
     std::jthread runtime_thread;
+    Channel<util::CameraTriggerEvent> camera_trigger_channel;
+    bool enable_trigger_sync { false };
+    std::uint64_t last_bound_trigger_seq_ { 0 };
 
     auto initialize(const YAML::Node& yaml) noexcept -> Result try {
         auto source = yaml["source"].as<std::string>();
@@ -61,6 +67,8 @@ struct Capturer::Impl {
             return std::unexpected { instantitation_result.error() };
         }
 
+        enable_trigger_sync = source == "hikcamera" && yaml["enable_trigger_sync"].as<bool>();
+
         auto show_loss_framerate          = yaml["show_loss_framerate"].as<bool>();
         auto show_loss_framerate_interval = yaml["show_loss_framerate_interval"].as<int>();
 
@@ -97,7 +105,43 @@ struct Capturer::Impl {
         log.info("[Capturer runtime thread] starts");
 
         // Success context
+        auto missing_trigger_limit        = util::TimesLimit { 3 };
+        auto last_image_capture_timestamp = std::optional<util::Clock::time_point> {};
+        auto bind_trigger_timestamp       = [&](std::unique_ptr<Image>& image) {
+            if (!enable_trigger_sync) {
+                return;
+            }
+
+            auto capture_timestamp = image->get_timestamp();
+            if (auto trigger = camera_trigger_channel.fetch_latest_matching(
+                    [&](const util::CameraTriggerEvent& candidate) {
+                        return candidate.seq > last_bound_trigger_seq_
+                            && (!last_image_capture_timestamp
+                                || candidate.timestamp > *last_image_capture_timestamp)
+                            && candidate.timestamp <= capture_timestamp
+                            && capture_timestamp - candidate.timestamp <= trigger_sync_max_age;
+                    })) {
+                image->set_timestamp(trigger->timestamp);
+                last_bound_trigger_seq_      = trigger->seq;
+                last_image_capture_timestamp = capture_timestamp;
+                missing_trigger_limit.reset();
+                missing_trigger_limit.enable();
+                return;
+            }
+
+            last_image_capture_timestamp = capture_timestamp;
+
+            if (missing_trigger_limit.tick()) {
+                log.warn("No camera trigger event is available for the captured image");
+            } else if (missing_trigger_limit.enabled()) {
+                missing_trigger_limit.disable();
+                log.warn(
+                    "{} times, stop printing trigger-sync warnings", missing_trigger_limit.count);
+            }
+        };
+
         auto success_callback = [&](std::unique_ptr<Image> image) {
+            bind_trigger_timestamp(image);
             auto newest = image.release();
             if (!capture_queue.push(newest)) {
 

src/kernel/feishu.hpp

@@ -2,60 +2,146 @@
 
 #include "utility/shared/context.hpp"
 #include "utility/shared/interprocess.hpp"
+#include <optional>
+#include <type_traits>
+#include <utility>
 
 namespace rmcs::kernel {
 
 template <typename T>
 constexpr const char* shm_name = nullptr;
 
+inline constexpr std::size_t kControlStateHistoryCapacity  = 4096;
+inline constexpr std::size_t kCameraTriggerHistoryCapacity = 512;
+
 template <>
 constexpr auto shm_name<util::AutoAimState> = "/shm_autoaim_state";
 
 template <>
 constexpr auto shm_name<util::ControlState> = "/shm_control_state";
 
+template <>
+constexpr auto shm_name<util::CameraTriggerEvent> = "/shm_camera_trigger";
+
 enum class RuntimeRole { AutoAim, Control };
 
-template <RuntimeRole Role>
-class Feishu {
-public:
-    using AutoAimState = util::AutoAimState;
-    using ControlState = util::ControlState;
+template <typename T>
+struct ChannelTraits {
+    using SendClient = typename rmcs::shm::Client<T>::Send;
+    using RecvClient = typename rmcs::shm::Client<T>::Recv;
+};
 
-    using SendData = std::conditional_t<Role == RuntimeRole::AutoAim, AutoAimState, ControlState>;
-    using RecvData = std::conditional_t<Role == RuntimeRole::AutoAim, ControlState, AutoAimState>;
+template <>
+struct ChannelTraits<util::ControlState> {
+    using SendClient =
+        typename rmcs::shm::HistoryClient<util::ControlState, kControlStateHistoryCapacity>::Send;
+    using RecvClient =
+        typename rmcs::shm::HistoryClient<util::ControlState, kControlStateHistoryCapacity>::Recv;
+};
 
-    using SendClient = rmcs::shm::Client<SendData>::Send;
-    using RecvClient = rmcs::shm::Client<RecvData>::Recv;
+template <>
+struct ChannelTraits<util::CameraTriggerEvent> {
+    using SendClient = typename rmcs::shm::HistoryClient<util::CameraTriggerEvent,
+        kCameraTriggerHistoryCapacity>::Send;
+    using RecvClient = typename rmcs::shm::HistoryClient<util::CameraTriggerEvent,
+        kCameraTriggerHistoryCapacity>::Recv;
+};
 
-    auto commit(SendData const& data) noexcept -> bool {
-        if (!ensure_open(send_client, shm_name<SendData>)) [[unlikely]]
+template <typename T>
+class Channel {
+public:
+    static_assert(shm_name<T> != nullptr, "Channel<T> requires shm_name<T> specialization");
+
+    using SendClient = typename ChannelTraits<T>::SendClient;
+    using RecvClient = typename ChannelTraits<T>::RecvClient;
+
+    auto commit(const T& data) noexcept -> bool {
+        if (!ensure_open(send_client_, shm_name<T>)) [[unlikely]]
             return false;
-        send_client.with_write([&](SendData& shared) { shared = data; });
-        return true;
+
+        if constexpr (requires { send_client_.push(data); }) {
+            return send_client_.push(data);
+        } else {
+            send_client_.with_write([&](T& shared) { shared = data; });
+            return true;
+        }
     }
 
-    auto fetch() noexcept -> const RecvData& {
-        // Note:直接读取当前共享内存中的数据；如需检测是否有新数据，请先调用 updated()
-        if (!ensure_open(recv_client, shm_name<RecvData>)) return recv_buffer;
-        recv_client.with_read([&](RecvData const& shared) { recv_buffer = shared; });
-        return recv_buffer;
+    auto fetch() noexcept -> const T& {
+        if (!ensure_open(recv_client_, shm_name<T>)) return recv_buffer_;
+
+        if constexpr (requires { recv_client_.latest(recv_buffer_); }) {
+            recv_client_.latest(recv_buffer_);
+        } else {
+            recv_client_.with_read([&](const T& shared) { recv_buffer_ = shared; });
+        }
+        return recv_buffer_;
     }
 
     auto updated() noexcept -> bool {
-        return ensure_open(recv_client, shm_name<RecvData>) && recv_client.is_updated();
+        return ensure_open(recv_client_, shm_name<T>) && recv_client_.is_updated();
     }
 
-private:
-    SendClient send_client {};
-    RecvClient recv_client {};
-
-    RecvData recv_buffer {};
+    template <typename Predicate>
+    auto fetch_latest_matching(Predicate&& predicate) noexcept -> std::optional<T> {
+        if (!ensure_open(recv_client_, shm_name<T>)) {
+            return std::nullopt;
+        }
+
+        auto buffer = T {};
+        if constexpr (requires {
+                          recv_client_.find_latest(std::forward<Predicate>(predicate), buffer);
+                      }) {
+            if (!recv_client_.find_latest(std::forward<Predicate>(predicate), buffer)) {
+                return std::nullopt;
+            }
+        } else {
+            return std::nullopt;
+        }
+
+        recv_buffer_ = buffer;
+        return buffer;
+    }
 
+private:
     template <typename Client>
-    auto ensure_open(Client& client, const char* name) noexcept -> bool {
+    static auto ensure_open(Client& client, const char* name) noexcept -> bool {
         return client.opened() || (name && client.open(name));
     }
+
+    SendClient send_client_ {};
+    RecvClient recv_client_ {};
+    T recv_buffer_ {};
+};
+
+template <RuntimeRole Role>
+class Feishu {
+public:
+    static_assert(Role == RuntimeRole::AutoAim || Role == RuntimeRole::Control,
+        "Feishu<Role> only supports AutoAim and Control");
+
+    using AutoAimState = util::AutoAimState;
+    using ControlState = util::ControlState;
+
+    using SendData = std::conditional_t<Role == RuntimeRole::AutoAim, AutoAimState, ControlState>;
+    using RecvData = std::conditional_t<Role == RuntimeRole::AutoAim, ControlState, AutoAimState>;
+
+    auto commit(SendData const& data) noexcept -> bool { return send_channel_.commit(data); }
+
+    auto fetch() noexcept -> const RecvData& { return recv_channel_.fetch(); }
+
+    auto updated() noexcept -> bool { return recv_channel_.updated(); }
+
+    template <RuntimeRole R = Role>
+    auto fetch_latest_before(util::Clock::time_point timestamp) noexcept
+        -> std::enable_if_t<R == RuntimeRole::AutoAim, std::optional<ControlState>> {
+        return recv_channel_.fetch_latest_matching(
+            [&](const ControlState& state) { return state.timestamp <= timestamp; });
+    }
+
+private:
+    Channel<SendData> send_channel_ {};
+    Channel<RecvData> recv_channel_ {};
 };
 
-}
+} // namespace rmcs::kernel