From 1938a053926ee9a2bd655d7a0b11fe03ab1a8703 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 16 Apr 2026 19:46:12 +0800 Subject: [PATCH 1/5] feat: implement camera hardware synchronization (WIP) --- config/config.yaml | 3 +- src/component.cpp | 46 +++++- src/kernel/capturer.cpp | 38 +++++ src/kernel/feishu.hpp | 173 +++++++++++++++++++---- src/runtime.cpp | 22 +-- src/utility/shared/context.hpp | 7 + src/utility/shared/interprocess.hpp | 210 +++++++++++++++++++++++++++- test/CMakeLists.txt | 6 + test/timestamp_alignment.cpp | 157 +++++++++++++++++++++ 9 files changed, 624 insertions(+), 38 deletions(-) create mode 100644 test/timestamp_alignment.cpp diff --git a/config/config.yaml b/config/config.yaml index c3b67eeb..92817946 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -6,6 +6,7 @@ capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 + enable_trigger_sync: false # hikcamera or local_video source: "hikcamera" hikcamera: @@ -95,6 +96,6 @@ fire_control: visualization: framerate: 60 - monitor_host: "127.0.0.1" + monitor_host: "192.168.2.154" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/component.cpp b/src/component.cpp index 3850cb5e..167c556c 100644 --- a/src/component.cpp +++ b/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,8 @@ 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"); } auto update() -> void override { @@ -56,8 +63,12 @@ class AutoAimComponent final : public rmcs_executor::Component { private: static constexpr auto auto_aim_state_timeout { std::chrono::milliseconds { 100 } }; + InputInterface predefined_timestamp_; InputInterface rmcs_tf; + InputInterface camera_trigger_seq_; + InputInterface camera_trigger_timestamp_; + double current_gimbal_yaw { std::numeric_limits::quiet_NaN() }; double current_gimbal_pitch { std::numeric_limits::quiet_NaN() }; @@ -65,9 +76,11 @@ class AutoAimComponent final : public rmcs_executor::Component { std::unique_ptr visual_odom_to_camera; Feishu feishu; + Channel 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 gimbal_takeover; OutputInterface shoot_permitted; @@ -133,6 +146,7 @@ class AutoAimComponent final : public rmcs_executor::Component { auto publish_control_state() -> void { update_gimbal_direction(); update_control_state(); + publish_camera_trigger_event(); auto success = feishu.commit(control_state); if (!success) { @@ -143,8 +157,38 @@ class AutoAimComponent final : public rmcs_executor::Component { } } + auto publish_camera_trigger_event() -> void { + auto trigger_seq = *camera_trigger_seq_; + 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("camera_trigger_gap_detected"); + } + + auto success = camera_trigger_channel.commit(CameraTriggerEvent { + .seq = trigger_seq, + .timestamp = *camera_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( diff --git a/src/kernel/capturer.cpp b/src/kernel/capturer.cpp index b545914a..0d554c93 100644 --- a/src/kernel/capturer.cpp +++ b/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" @@ -23,9 +24,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 capture_queue; std::jthread runtime_thread; + Channel 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(); @@ -61,6 +66,9 @@ struct Capturer::Impl { return std::unexpected { instantitation_result.error() }; } + auto trigger_sync_config = yaml["enable_trigger_sync"].as(); + enable_trigger_sync = (source == "hikcamera" && trigger_sync_config); + auto show_loss_framerate = yaml["show_loss_framerate"].as(); auto show_loss_framerate_interval = yaml["show_loss_framerate_interval"].as(); @@ -97,7 +105,37 @@ struct Capturer::Impl { log.info("[Capturer runtime thread] starts"); // Success context + auto missing_trigger_limit = util::TimesLimit { 3 }; + auto bind_trigger_timestamp = [&](std::unique_ptr& 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_ + && candidate.timestamp <= capture_timestamp + && capture_timestamp - candidate.timestamp <= trigger_sync_max_age; + })) { + image->set_timestamp(trigger->timestamp); + last_bound_trigger_seq_ = trigger->seq; + missing_trigger_limit.reset(); + missing_trigger_limit.enable(); + return; + } + + 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) { + bind_trigger_timestamp(image); auto newest = image.release(); if (!capture_queue.push(newest)) { diff --git a/src/kernel/feishu.hpp b/src/kernel/feishu.hpp index de74f918..e243b11e 100644 --- a/src/kernel/feishu.hpp +++ b/src/kernel/feishu.hpp @@ -2,60 +2,183 @@ #include "utility/shared/context.hpp" #include "utility/shared/interprocess.hpp" +#include +#include +#include +#include namespace rmcs::kernel { template 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 = "/shm_autoaim_state"; template <> constexpr auto shm_name = "/shm_control_state"; +template <> +constexpr auto shm_name = "/shm_camera_trigger"; + enum class RuntimeRole { AutoAim, Control }; -template -class Feishu { -public: - using AutoAimState = util::AutoAimState; - using ControlState = util::ControlState; +template +struct ChannelTraits { + using SendClient = typename rmcs::shm::Client::Send; + using RecvClient = typename rmcs::shm::Client::Recv; +}; - using SendData = std::conditional_t; - using RecvData = std::conditional_t; +template <> +struct ChannelTraits { + using SendClient = + typename rmcs::shm::HistoryClient::Send; + using RecvClient = + typename rmcs::shm::HistoryClient::Recv; +}; - using SendClient = rmcs::shm::Client::Send; - using RecvClient = rmcs::shm::Client::Recv; +template <> +struct ChannelTraits { + using SendClient = typename rmcs::shm::HistoryClient::Send; + using RecvClient = typename rmcs::shm::HistoryClient::Recv; +}; + +namespace detail { + + template + auto channel_write(Client& client, const T& data) noexcept -> bool { + if constexpr (requires { client.push(data); }) { + return client.push(data); + } else { + client.with_write([&](T& shared) { shared = data; }); + return true; + } + } + + template + auto channel_read_latest(Client& client, T& buffer) noexcept -> bool { + if constexpr (requires { client.latest(buffer); }) { + return client.latest(buffer); + } else { + client.with_read([&](const T& shared) { buffer = shared; }); + return true; + } + } - auto commit(SendData const& data) noexcept -> bool { - if (!ensure_open(send_client, shm_name)) [[unlikely]] + template + auto channel_read_latest_matching(Client& client, Predicate&& predicate, T& buffer) noexcept + -> bool { + if constexpr (requires { + client.find_latest(std::forward(predicate), buffer); + }) { + return client.find_latest(std::forward(predicate), buffer); + } else { return false; - send_client.with_write([&](SendData& shared) { shared = data; }); - return true; + } } - auto fetch() noexcept -> const RecvData& { - // Note:直接读取当前共享内存中的数据；如需检测是否有新数据，请先调用 updated() - if (!ensure_open(recv_client, shm_name)) return recv_buffer; - recv_client.with_read([&](RecvData const& shared) { recv_buffer = shared; }); - return recv_buffer; + template + auto channel_pop_next(Client& client, T& buffer) noexcept -> bool { + if constexpr (requires { client.pop_next(buffer); }) { + return client.pop_next(buffer); + } else { + return false; + } + } + +} // namespace detail + +template +class Channel { +public: + using SendClient = typename ChannelTraits::SendClient; + using RecvClient = typename ChannelTraits::RecvClient; + + auto commit(const T& data) noexcept -> bool { + if (!ensure_open(send_client_, shm_name)) [[unlikely]] + return false; + return detail::channel_write(send_client_, data); + } + + auto fetch() noexcept -> const T& { + if (!ensure_open(recv_client_, shm_name)) return recv_buffer_; + std::ignore = detail::channel_read_latest(recv_client_, recv_buffer_); + return recv_buffer_; } auto updated() noexcept -> bool { - return ensure_open(recv_client, shm_name) && recv_client.is_updated(); + return ensure_open(recv_client_, shm_name) && recv_client_.is_updated(); } -private: - SendClient send_client {}; - RecvClient recv_client {}; + template + auto fetch_latest_matching(Predicate&& predicate) noexcept -> std::optional { + if (!ensure_open(recv_client_, shm_name)) { + return std::nullopt; + } + + auto buffer = T {}; + if (!detail::channel_read_latest_matching( + recv_client_, std::forward(predicate), buffer)) { + return std::nullopt; + } + recv_buffer_ = buffer; + return buffer; + } - RecvData recv_buffer {}; + auto pop_next() noexcept -> std::optional { + if (!ensure_open(recv_client_, shm_name)) { + return std::nullopt; + } + + auto buffer = T {}; + if (!detail::channel_pop_next(recv_client_, buffer)) { + return std::nullopt; + } + recv_buffer_ = buffer; + return buffer; + } +private: template - 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 +class Feishu { +public: + using AutoAimState = util::AutoAimState; + using ControlState = util::ControlState; + + using SendData = std::conditional_t; + using RecvData = std::conditional_t; + + 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 + auto fetch_latest_before(util::Clock::time_point timestamp) noexcept + -> std::enable_if_t> { + return recv_channel_.fetch_latest_matching( + [&](const ControlState& state) { return state.timestamp <= timestamp; }); + } + +private: + Channel send_channel_ {}; + Channel recv_channel_ {}; }; -} +} // namespace rmcs::kernel diff --git a/src/runtime.cpp b/src/runtime.cpp index d2d2f2d2..2134d772 100644 --- a/src/runtime.cpp +++ b/src/runtime.cpp @@ -14,8 +14,8 @@ #include "utility/rclcpp/parameters.hpp" #include "utility/singleton/running.hpp" -#include #include +#include #include #include #include @@ -27,11 +27,11 @@ using namespace rmcs::kernel; auto main() -> int { using namespace std::chrono_literals; - std::signal(SIGINT, [](int) { util::set_running(false); }); - auto rclcpp_node = util::RclcppNode { "AutoAim" }; rclcpp_node.set_pub_topic_prefix("/rmcs/auto_aim/"); + rclcpp::on_shutdown([] { util::set_running(false); }); + { /// Runtime auto feishu = kernel::Feishu {}; @@ -112,18 +112,20 @@ auto main() -> int { /// /// Steps /// - const auto fetch_control_state = [&] -> ControlState { + const auto fetch_control_state = [&](Clock::time_point image_timestamp) -> ControlState { if (is_local_runtime) { auto state = ControlState {}; state.reset(); return state; } - if (!feishu.updated()) { - action_throttler.dispatch(control_state_label, - [&] { rclcpp_node.warn("Control state 尚未更新，使用上一次缓存值."); }); - } else { + + if (auto state = feishu.fetch_latest_before(image_timestamp)) { action_throttler.reset(control_state_label); + return *state; } + + action_throttler.dispatch(control_state_label, + [&] { rclcpp_node.warn("Control state history 不可用，使用当前最新缓存值."); }); return feishu.fetch(); }; @@ -141,7 +143,7 @@ auto main() -> int { }; for (;;) { - if (!util::get_running()) [[unlikely]] + if (!util::get_running() || !rclcpp::ok()) [[unlikely]] break; rclcpp_node.spin_once(); @@ -154,7 +156,7 @@ auto main() -> int { } }; std::ignore = stream_guard; - auto control_state = fetch_control_state(); + auto control_state = fetch_control_state(image->get_timestamp()); auto next_state = AutoAimState {}; next_state.reset(); diff --git a/src/utility/shared/context.hpp b/src/utility/shared/context.hpp index a0654089..da021bde 100644 --- a/src/utility/shared/context.hpp +++ b/src/utility/shared/context.hpp @@ -4,6 +4,7 @@ #include "utility/math/linear.hpp" #include "utility/robot/id.hpp" #include +#include #include namespace rmcs::util { @@ -70,6 +71,12 @@ struct AutoAimState { }; static_assert(std::is_trivially_copyable_v); +struct CameraTriggerEvent { + std::uint64_t seq {}; + Clock::time_point timestamp {}; +}; +static_assert(std::is_trivially_copyable_v); + struct ControlState { Clock::time_point timestamp {}; ShootMode shoot_mode { ShootMode::BATTLE }; diff --git a/src/utility/shared/interprocess.hpp b/src/utility/shared/interprocess.hpp index 98c4584d..085d2d71 100644 --- a/src/utility/shared/interprocess.hpp +++ b/src/utility/shared/interprocess.hpp @@ -1,8 +1,13 @@ #pragma once +#include #include +#include #include +#include #include #include +#include +#include namespace rmcs::shm { @@ -155,4 +160,207 @@ struct Client { }; }; -} +template +struct HistoryClient { + static_assert(N > 0, "History capacity must be non-zero"); + static_assert(std::is_trivially_copyable_v, "T must be trivially copyable"); + + struct alignas(64) Entry final { + alignas(64) std::atomic version; + std::uint64_t sequence; + T data; + }; + + struct alignas(64) Context final { + alignas(64) std::atomic committed; + alignas(64) std::array entries; + }; + + static constexpr auto kContextLen = sizeof(Context); + + class Send final { + public: + ~Send() noexcept { + if (context) { + munmap(static_cast(context), kContextLen); + } + if (shm_fd != -1) { + close(shm_fd); + } + } + + auto open(const char* id) noexcept -> bool { + shm_fd = shm_open(id, O_CREAT | O_RDWR, 0666); + if (shm_fd == -1) { + return false; + } + if (ftruncate(shm_fd, kContextLen) == -1) { + close(shm_fd); + return false; + } + + auto* shm_ptr = + mmap(nullptr, kContextLen, PROT_WRITE | PROT_READ, MAP_SHARED, shm_fd, 0); + if (shm_ptr == MAP_FAILED) { + close(shm_fd); + return false; + } + + context = static_cast(shm_ptr); + next_sequence = context->committed.load(std::memory_order::acquire); + return true; + } + + auto opened() const noexcept { return context != nullptr; } + + auto push(const T& data) noexcept -> bool { + if (!context) return false; + + const auto sequence = next_sequence++; + auto& entry = context->entries[sequence % N]; + + entry.version.fetch_add(1, std::memory_order::acq_rel); + entry.sequence = sequence; + entry.data = data; + entry.version.fetch_add(1, std::memory_order::acq_rel); + + context->committed.store(next_sequence, std::memory_order::release); + return true; + } + + private: + int shm_fd { -1 }; + Context* context { nullptr }; + std::uint64_t next_sequence { 0 }; + }; + + class Recv final { + public: + ~Recv() noexcept { + if (context) { + munmap(static_cast(context), kContextLen); + } + if (shm_fd != -1) { + close(shm_fd); + } + } + + auto open(const char* id) noexcept -> bool { + shm_fd = shm_open(id, O_RDWR, 0666); + if (shm_fd == -1) { + return false; + } + + auto* shm_ptr = + mmap(nullptr, kContextLen, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + if (shm_ptr == MAP_FAILED) { + close(shm_fd); + return false; + } + + context = static_cast(shm_ptr); + const auto committed = context->committed.load(std::memory_order::acquire); + observed_committed = 0; + next_sequence_to_pop_ = committed; + return true; + } + + auto opened() const noexcept { return context != nullptr; } + + auto is_updated() const noexcept -> bool { + if (!context) return false; + return context->committed.load(std::memory_order::acquire) != observed_committed; + } + + auto latest(T& out_data) const noexcept -> bool { + return find_latest([](const T&) { return true; }, out_data); + } + + template + auto find_latest(Predicate&& predicate, T& out_data) const noexcept -> bool { + if (!context) return false; + + const auto committed = context->committed.load(std::memory_order::acquire); + const auto oldest = oldest_sequence(committed); + + for (auto sequence = committed; sequence > oldest; --sequence) { + auto candidate = T {}; + if (!read_sequence(sequence - 1, candidate)) { + continue; + } + if (predicate(candidate)) { + out_data = candidate; + observed_committed = committed; + return true; + } + } + + observed_committed = committed; + return false; + } + + auto pop_next(T& out_data) const noexcept -> bool { + if (!context) return false; + + const auto committed = context->committed.load(std::memory_order::acquire); + const auto oldest = oldest_sequence(committed); + + if (next_sequence_to_pop_ < oldest) { + next_sequence_to_pop_ = oldest; + observed_committed = committed; + return false; + } + + if (next_sequence_to_pop_ >= committed) { + observed_committed = committed; + return false; + } + + if (!read_sequence(next_sequence_to_pop_, out_data)) { + return false; + } + + ++next_sequence_to_pop_; + observed_committed = committed; + return true; + } + + private: + static auto oldest_sequence(std::uint64_t committed) noexcept -> std::uint64_t { + return committed > N ? committed - N : 0; + } + + auto read_sequence(std::uint64_t sequence, T& out_data) const noexcept -> bool { + if (!context) return false; + + const auto& entry = context->entries[sequence % N]; + + auto version1 = std::uint64_t {}; + auto version2 = std::uint64_t {}; + auto stored_sequence = std::uint64_t {}; + auto candidate = T {}; + + do { + version1 = entry.version.load(std::memory_order::acquire); + stored_sequence = entry.sequence; + candidate = entry.data; + version2 = entry.version.load(std::memory_order::acquire); + } while ((version1 != version2) || (version1 & 1)); + + if (stored_sequence != sequence) { + return false; + } + + out_data = candidate; + return true; + } + + mutable std::uint64_t observed_committed { 0 }; + mutable std::uint64_t next_sequence_to_pop_ { 0 }; + + int shm_fd { -1 }; + Context* context { nullptr }; + }; +}; + +} // namespace rmcs::shm diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f2c3fdb0..ae37faea 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -121,6 +121,12 @@ ament_add_gtest( ${TEST_DIR}/feishu_test.cpp ) +# Timestamp alignment +ament_add_gtest( + test_timestamp_alignment + ${TEST_DIR}/timestamp_alignment.cpp +) + # Action throttler ament_add_gtest( test_action_throttler diff --git a/test/timestamp_alignment.cpp b/test/timestamp_alignment.cpp new file mode 100644 index 00000000..0ff7fdde --- /dev/null +++ b/test/timestamp_alignment.cpp @@ -0,0 +1,157 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "kernel/feishu.hpp" +#include "utility/shared/interprocess.hpp" + +namespace { + +using namespace std::chrono_literals; + +using rmcs::kernel::Feishu; +using rmcs::kernel::RuntimeRole; +using rmcs::shm::HistoryClient; +using rmcs::util::CameraTriggerEvent; +using rmcs::util::Clock; +using rmcs::util::ControlState; + +struct ShmScope { + explicit ShmScope(std::string name) + : name_(std::move(name)) { + (void)::shm_unlink(name_.c_str()); + } + + ~ShmScope() { (void)::shm_unlink(name_.c_str()); } + + [[nodiscard]] auto c_str() const noexcept -> const char* { return name_.c_str(); } + +private: + std::string name_; +}; + +struct FeishuShmScope { + FeishuShmScope() { + for (auto name : kNames) { + (void)::shm_unlink(name); + } + } + + ~FeishuShmScope() { + for (auto name : kNames) { + (void)::shm_unlink(name); + } + } + +private: + static constexpr std::array kNames { + rmcs::kernel::shm_name, + rmcs::kernel::shm_name, + rmcs::kernel::shm_name, + }; +}; + +auto unique_shm_name(const char* prefix) -> std::string { + static auto counter = std::atomic { 0 }; + return std::string { "/" } + prefix + "_" + std::to_string(::getpid()) + "_" + + std::to_string(counter.fetch_add(1, std::memory_order_relaxed)); +} + +auto make_control_state(Clock::time_point timestamp) -> ControlState { + auto state = ControlState {}; + state.timestamp = timestamp; + return state; +} + +TEST(TimestampAlignment, ClockDomainUsesSteadyClock) { + EXPECT_TRUE((std::is_same_v)); +} + +TEST(TimestampAlignment, HistoryClientPopNextConsumesCameraTriggersInOrder) { + using Channel = HistoryClient; + + auto shm_name = ShmScope { unique_shm_name("rmcs_auto_aim_trigger_fifo") }; + auto send = Channel::Send {}; + auto recv = Channel::Recv {}; + + ASSERT_TRUE(send.open(shm_name.c_str())); + ASSERT_TRUE(recv.open(shm_name.c_str())); + + auto base = Clock::now(); + ASSERT_TRUE(send.push(CameraTriggerEvent { .seq = 11, .timestamp = base + 1ms })); + ASSERT_TRUE(send.push(CameraTriggerEvent { .seq = 12, .timestamp = base + 2ms })); + ASSERT_TRUE(send.push(CameraTriggerEvent { .seq = 13, .timestamp = base + 3ms })); + + auto event = CameraTriggerEvent {}; + ASSERT_TRUE(recv.pop_next(event)); + EXPECT_EQ(event.seq, 11U); + EXPECT_EQ(event.timestamp, base + 1ms); + + ASSERT_TRUE(recv.pop_next(event)); + EXPECT_EQ(event.seq, 12U); + EXPECT_EQ(event.timestamp, base + 2ms); + + ASSERT_TRUE(recv.pop_next(event)); + EXPECT_EQ(event.seq, 13U); + EXPECT_EQ(event.timestamp, base + 3ms); + + EXPECT_FALSE(recv.pop_next(event)); +} + +TEST(TimestampAlignment, HistoryClientFindLatestChoosesNewestStateNotAfterImageTimestamp) { + using Channel = HistoryClient; + + auto shm_name = ShmScope { unique_shm_name("rmcs_auto_aim_control_history") }; + auto send = Channel::Send {}; + auto recv = Channel::Recv {}; + + ASSERT_TRUE(send.open(shm_name.c_str())); + ASSERT_TRUE(recv.open(shm_name.c_str())); + + auto base = Clock::now(); + ASSERT_TRUE(send.push(make_control_state(base + 10ms))); + ASSERT_TRUE(send.push(make_control_state(base + 20ms))); + ASSERT_TRUE(send.push(make_control_state(base + 30ms))); + + auto matched = ControlState {}; + ASSERT_TRUE(recv.find_latest( + [&](const ControlState& state) { return state.timestamp <= base + 25ms; }, matched)); + EXPECT_EQ(matched.timestamp, base + 20ms); + + ASSERT_TRUE(recv.find_latest( + [&](const ControlState& state) { return state.timestamp <= base + 10ms; }, matched)); + EXPECT_EQ(matched.timestamp, base + 10ms); + + EXPECT_FALSE( + recv.find_latest([&](const ControlState& state) { return state.timestamp < base; }, matched)); +} + +TEST(TimestampAlignment, FeishuFetchLatestBeforeUsesControlStateHistorySemantics) { + auto shm_scope = FeishuShmScope {}; + auto control = Feishu {}; + auto auto_aim = Feishu {}; + + auto base = Clock::now(); + ASSERT_TRUE(control.commit(make_control_state(base + 10ms))); + ASSERT_TRUE(control.commit(make_control_state(base + 20ms))); + ASSERT_TRUE(control.commit(make_control_state(base + 30ms))); + + auto matched = auto_aim.fetch_latest_before(base + 25ms); + ASSERT_TRUE(matched.has_value()); + EXPECT_EQ(matched->timestamp, base + 20ms); + + matched = auto_aim.fetch_latest_before(base + 30ms); + ASSERT_TRUE(matched.has_value()); + EXPECT_EQ(matched->timestamp, base + 30ms); + + EXPECT_FALSE(auto_aim.fetch_latest_before(base + 5ms).has_value()); +} + +} // namespace From d1c4e04dcfae82a96bcdd3ee12dd35cc10776ba8 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 16 Apr 2026 21:22:04 +0800 Subject: [PATCH 2/5] refactor: refactor IPC module for better message handling efficiency --- config/config.yaml | 2 +- src/kernel/feishu.hpp | 87 +++-------- src/utility/shared/interprocess.hpp | 229 +++++++++++++--------------- 3 files changed, 128 insertions(+), 190 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index 92817946..d24c36b0 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -96,6 +96,6 @@ fire_control: visualization: framerate: 60 - monitor_host: "192.168.2.154" + monitor_host: "127.0.0.1" monitor_port: "5000" stream_type: "RTP_JEPG" diff --git a/src/kernel/feishu.hpp b/src/kernel/feishu.hpp index e243b11e..97619cef 100644 --- a/src/kernel/feishu.hpp +++ b/src/kernel/feishu.hpp @@ -3,7 +3,6 @@ #include "utility/shared/context.hpp" #include "utility/shared/interprocess.hpp" #include -#include #include #include @@ -48,66 +47,34 @@ struct ChannelTraits { kCameraTriggerHistoryCapacity>::Recv; }; -namespace detail { - - template - auto channel_write(Client& client, const T& data) noexcept -> bool { - if constexpr (requires { client.push(data); }) { - return client.push(data); - } else { - client.with_write([&](T& shared) { shared = data; }); - return true; - } - } - - template - auto channel_read_latest(Client& client, T& buffer) noexcept -> bool { - if constexpr (requires { client.latest(buffer); }) { - return client.latest(buffer); - } else { - client.with_read([&](const T& shared) { buffer = shared; }); - return true; - } - } - - template - auto channel_read_latest_matching(Client& client, Predicate&& predicate, T& buffer) noexcept - -> bool { - if constexpr (requires { - client.find_latest(std::forward(predicate), buffer); - }) { - return client.find_latest(std::forward(predicate), buffer); - } else { - return false; - } - } - - template - auto channel_pop_next(Client& client, T& buffer) noexcept -> bool { - if constexpr (requires { client.pop_next(buffer); }) { - return client.pop_next(buffer); - } else { - return false; - } - } - -} // namespace detail - template class Channel { public: + static_assert(shm_name != nullptr, "Channel requires shm_name specialization"); + using SendClient = typename ChannelTraits::SendClient; using RecvClient = typename ChannelTraits::RecvClient; auto commit(const T& data) noexcept -> bool { if (!ensure_open(send_client_, shm_name)) [[unlikely]] return false; - return detail::channel_write(send_client_, data); + + 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 T& { if (!ensure_open(recv_client_, shm_name)) return recv_buffer_; - std::ignore = detail::channel_read_latest(recv_client_, 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_; } @@ -122,23 +89,16 @@ class Channel { } auto buffer = T {}; - if (!detail::channel_read_latest_matching( - recv_client_, std::forward(predicate), buffer)) { - return std::nullopt; - } - recv_buffer_ = buffer; - return buffer; - } - - auto pop_next() noexcept -> std::optional { - if (!ensure_open(recv_client_, shm_name)) { + if constexpr (requires { + recv_client_.find_latest(std::forward(predicate), buffer); + }) { + if (!recv_client_.find_latest(std::forward(predicate), buffer)) { + return std::nullopt; + } + } else { return std::nullopt; } - auto buffer = T {}; - if (!detail::channel_pop_next(recv_client_, buffer)) { - return std::nullopt; - } recv_buffer_ = buffer; return buffer; } @@ -157,6 +117,9 @@ class Channel { template class Feishu { public: + static_assert(Role == RuntimeRole::AutoAim || Role == RuntimeRole::Control, + "Feishu only supports AutoAim and Control"); + using AutoAimState = util::AutoAimState; using ControlState = util::ControlState; diff --git a/src/utility/shared/interprocess.hpp b/src/utility/shared/interprocess.hpp index 085d2d71..df727c8a 100644 --- a/src/utility/shared/interprocess.hpp +++ b/src/utility/shared/interprocess.hpp @@ -1,8 +1,8 @@ #pragma once #include #include -#include #include +#include #include #include #include @@ -11,6 +11,65 @@ namespace rmcs::shm { +namespace detail { + + template + class MappedContext final { + public: + MappedContext() = default; + MappedContext(const MappedContext&) = delete; + auto operator=(const MappedContext&) -> MappedContext& = delete; + + ~MappedContext() noexcept { + if (context_ != nullptr) { + munmap(static_cast(context_), Len); + } + if (shm_fd_ != -1) { + close(shm_fd_); + } + } + + auto open_sender(const char* id) noexcept -> bool { + return open(id, O_CREAT | O_RDWR, true); + } + + auto open_receiver(const char* id) noexcept -> bool { return open(id, O_RDWR, false); } + + auto opened() const noexcept -> bool { return context_ != nullptr; } + + auto get() const noexcept -> Context* { return context_; } + + private: + auto open(const char* id, int flags, bool resize) noexcept -> bool { + if (opened()) return true; + + auto fd = shm_open(id, flags, 0666); + if (fd == -1) { + return false; + } + + if (resize && ftruncate(fd, Len) == -1) { + close(fd); + return false; + } + + auto* shm_ptr = mmap(nullptr, Len, PROT_WRITE | PROT_READ, MAP_SHARED, fd, 0); + if (shm_ptr == MAP_FAILED) { + close(fd); + return false; + } + + shm_fd_ = fd; + context_ = static_cast(shm_ptr); + return true; + } + + int shm_fd_ { -1 }; + Context* context_ { nullptr }; + }; + +} // namespace detail + template struct alignas(64) SharedContext final { @@ -26,50 +85,24 @@ struct Client { using Context = SharedContext; static constexpr auto kContextLen = sizeof(Context); - static constexpr auto kDataLen = sizeof(T); class Send final { public: - ~Send() noexcept { - if (context) { - munmap(static_cast(context), kContextLen); - } - if (shm_fd != -1) { - close(shm_fd); - } - } - auto open(const char* id) noexcept -> bool { - shm_fd = shm_open(id, O_CREAT | O_RDWR, 0666); - if (shm_fd == -1) { - return false; - } - if (ftruncate(shm_fd, kContextLen) == -1) { - close(shm_fd); - return false; - } + Send() = default; + Send(const Send&) = delete; + auto operator=(const Send&) -> Send& = delete; - auto* shm_ptr = - mmap(nullptr, kContextLen, PROT_WRITE | PROT_READ, MAP_SHARED, shm_fd, 0); - if (shm_ptr == MAP_FAILED) { - close(shm_fd); - return false; - } - - context = static_cast(shm_ptr); - return true; - } - auto opened() const noexcept { return context != nullptr; } + auto open(const char* id) noexcept -> bool { return context_.open_sender(id); } + auto opened() const noexcept { return context_.opened(); } auto send(const T& data) noexcept -> void { - if (!context) return; - context->version.fetch_add(1, std::memory_order::acq_rel); - context->data = data; - context->version.fetch_add(1, std::memory_order::acq_rel); + with_write([&](T& shared) { shared = data; }); } template auto with_write(F&& fn) noexcept -> void requires std::invocable { + auto* context = context_.get(); if (!context) return; context->version.fetch_add(1, std::memory_order::acq_rel); @@ -78,57 +111,26 @@ struct Client { } private: - int shm_fd { -1 }; - Context* context { nullptr }; + detail::MappedContext context_ {}; }; class Recv { public: - ~Recv() noexcept { - if (context) { - munmap(static_cast(context), kContextLen); - } - if (shm_fd != -1) { - close(shm_fd); - } - } - - auto open(const char* id) noexcept -> bool { - shm_fd = shm_open(id, O_RDWR, 0666); - if (shm_fd == -1) { - return false; - } + Recv() = default; + Recv(const Recv&) = delete; + auto operator=(const Recv&) -> Recv& = delete; - auto* shm_ptr = - mmap(nullptr, kContextLen, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); - if (shm_ptr == MAP_FAILED) { - close(shm_fd); - return false; - } - - context = static_cast(shm_ptr); - return true; - } - auto opened() const noexcept { return context != nullptr; } + auto open(const char* id) noexcept -> bool { return context_.open_receiver(id); } + auto opened() const noexcept { return context_.opened(); } auto recv(T& out_data) const noexcept -> void { - if (!context) return; - - auto version1 = std::uint64_t {}; - auto version2 = std::uint64_t {}; - - do { - version1 = context->version.load(std::memory_order::acquire); - out_data = context->data; - version2 = context->version.load(std::memory_order::acquire); - } while ((version1 != version2) || (version1 & 1)); - - version = version2; + with_read([&](const T& shared) { out_data = shared; }); } template auto with_read(F&& fn) const noexcept -> void requires std::invocable { + auto* context = context_.get(); if (!context) return; auto snapshot = T {}; @@ -146,6 +148,7 @@ struct Client { } auto is_updated() const noexcept -> bool { + auto* context = context_.get(); if (!context) return false; auto current = context->version.load(std::memory_order::acquire); @@ -155,8 +158,7 @@ struct Client { private: mutable std::uint64_t version { 0 }; - int shm_fd { -1 }; - Context* context { nullptr }; + detail::MappedContext context_ {}; }; }; @@ -180,40 +182,24 @@ struct HistoryClient { class Send final { public: - ~Send() noexcept { - if (context) { - munmap(static_cast(context), kContextLen); - } - if (shm_fd != -1) { - close(shm_fd); - } - } + Send() = default; + Send(const Send&) = delete; + auto operator=(const Send&) -> Send& = delete; auto open(const char* id) noexcept -> bool { - shm_fd = shm_open(id, O_CREAT | O_RDWR, 0666); - if (shm_fd == -1) { - return false; - } - if (ftruncate(shm_fd, kContextLen) == -1) { - close(shm_fd); + if (!context_.open_sender(id)) { return false; } - auto* shm_ptr = - mmap(nullptr, kContextLen, PROT_WRITE | PROT_READ, MAP_SHARED, shm_fd, 0); - if (shm_ptr == MAP_FAILED) { - close(shm_fd); - return false; - } - - context = static_cast(shm_ptr); + auto* context = context_.get(); next_sequence = context->committed.load(std::memory_order::acquire); return true; } - auto opened() const noexcept { return context != nullptr; } + auto opened() const noexcept { return context_.opened(); } auto push(const T& data) noexcept -> bool { + auto* context = context_.get(); if (!context) return false; const auto sequence = next_sequence++; @@ -229,45 +215,32 @@ struct HistoryClient { } private: - int shm_fd { -1 }; - Context* context { nullptr }; + detail::MappedContext context_ {}; std::uint64_t next_sequence { 0 }; }; class Recv final { public: - ~Recv() noexcept { - if (context) { - munmap(static_cast(context), kContextLen); - } - if (shm_fd != -1) { - close(shm_fd); - } - } + Recv() = default; + Recv(const Recv&) = delete; + auto operator=(const Recv&) -> Recv& = delete; auto open(const char* id) noexcept -> bool { - shm_fd = shm_open(id, O_RDWR, 0666); - if (shm_fd == -1) { - return false; - } - - auto* shm_ptr = - mmap(nullptr, kContextLen, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); - if (shm_ptr == MAP_FAILED) { - close(shm_fd); + if (!context_.open_receiver(id)) { return false; } - context = static_cast(shm_ptr); + auto* context = context_.get(); const auto committed = context->committed.load(std::memory_order::acquire); observed_committed = 0; next_sequence_to_pop_ = committed; return true; } - auto opened() const noexcept { return context != nullptr; } + auto opened() const noexcept { return context_.opened(); } auto is_updated() const noexcept -> bool { + auto* context = context_.get(); if (!context) return false; return context->committed.load(std::memory_order::acquire) != observed_committed; } @@ -278,6 +251,7 @@ struct HistoryClient { template auto find_latest(Predicate&& predicate, T& out_data) const noexcept -> bool { + auto* context = context_.get(); if (!context) return false; const auto committed = context->committed.load(std::memory_order::acquire); @@ -289,8 +263,8 @@ struct HistoryClient { continue; } if (predicate(candidate)) { - out_data = candidate; - observed_committed = committed; + out_data = candidate; + observed_committed = committed; return true; } } @@ -300,6 +274,7 @@ struct HistoryClient { } auto pop_next(T& out_data) const noexcept -> bool { + auto* context = context_.get(); if (!context) return false; const auto committed = context->committed.load(std::memory_order::acquire); @@ -331,14 +306,15 @@ struct HistoryClient { } auto read_sequence(std::uint64_t sequence, T& out_data) const noexcept -> bool { + auto* context = context_.get(); if (!context) return false; const auto& entry = context->entries[sequence % N]; - auto version1 = std::uint64_t {}; - auto version2 = std::uint64_t {}; - auto stored_sequence = std::uint64_t {}; - auto candidate = T {}; + auto version1 = std::uint64_t {}; + auto version2 = std::uint64_t {}; + auto stored_sequence = std::uint64_t {}; + auto candidate = T {}; do { version1 = entry.version.load(std::memory_order::acquire); @@ -358,8 +334,7 @@ struct HistoryClient { mutable std::uint64_t observed_committed { 0 }; mutable std::uint64_t next_sequence_to_pop_ { 0 }; - int shm_fd { -1 }; - Context* context { nullptr }; + detail::MappedContext context_ {}; }; }; From eb1fcb0b63ddaa8c0139da612d51c6df3202ee1f Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 16 Apr 2026 22:13:03 +0800 Subject: [PATCH 3/5] chore(config): enable fixed_framerate and trigger_sync --- config/config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/config.yaml b/config/config.yaml index d24c36b0..d31c439b 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -6,7 +6,7 @@ capturer: show_loss_framerate: false show_loss_framerate_interval: 500 reconnect_wait_interval: 100 - enable_trigger_sync: false + enable_trigger_sync: true # hikcamera or local_video source: "hikcamera" hikcamera: @@ -22,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" From e75b44bb480872ce17bb603195e4748d90ab52a6 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 23 Apr 2026 18:57:44 +0800 Subject: [PATCH 4/5] fix: correct minor timestamp alignment offset between image and IMU --- src/component.cpp | 25 +++++++--- src/kernel/capturer.cpp | 7 +++ src/module/predictor/outpost/robot_state.cpp | 4 ++ src/module/predictor/regular/robot_state.cpp | 4 ++ test/timestamp_alignment.cpp | 51 ++++++++++++++++++++ 5 files changed, 85 insertions(+), 6 deletions(-) diff --git a/src/component.cpp b/src/component.cpp index 167c556c..9b849a7e 100644 --- a/src/component.cpp +++ b/src/component.cpp @@ -48,6 +48,7 @@ class AutoAimComponent final : public rmcs_executor::Component { 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 { @@ -146,19 +147,31 @@ class AutoAimComponent final : public rmcs_executor::Component { auto publish_control_state() -> void { update_gimbal_direction(); update_control_state(); - publish_camera_trigger_event(); auto success = feishu.commit(control_state); if (!success) { action_throttler.dispatch("commit_control_state_failed", [&] { rclcpp.info("commit control state failed!"); }); - } else { - action_throttler.reset("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() -> void { - auto trigger_seq = *camera_trigger_seq_; + 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; } @@ -175,7 +188,7 @@ class AutoAimComponent final : public rmcs_executor::Component { auto success = camera_trigger_channel.commit(CameraTriggerEvent { .seq = trigger_seq, - .timestamp = *camera_trigger_timestamp_, + .timestamp = trigger_timestamp, }); if (!success) { action_throttler.dispatch("commit_camera_trigger_failed", diff --git a/src/kernel/capturer.cpp b/src/kernel/capturer.cpp index 0d554c93..86c53615 100644 --- a/src/kernel/capturer.cpp +++ b/src/kernel/capturer.cpp @@ -9,6 +9,7 @@ #include "utility/thread/spsc_queue.hpp" #include "utility/times_limit.hpp" +#include #include #include @@ -106,6 +107,7 @@ struct Capturer::Impl { // Success context auto missing_trigger_limit = util::TimesLimit { 3 }; + auto last_image_capture_timestamp = std::optional {}; auto bind_trigger_timestamp = [&](std::unique_ptr& image) { if (!enable_trigger_sync) { return; @@ -115,16 +117,21 @@ struct Capturer::Impl { 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()) { diff --git a/src/module/predictor/outpost/robot_state.cpp b/src/module/predictor/outpost/robot_state.cpp index 2af4aa5f..7652b69d 100644 --- a/src/module/predictor/outpost/robot_state.cpp +++ b/src/module/predictor/outpost/robot_state.cpp @@ -315,6 +315,10 @@ struct OutpostRobotState::Impl { } auto predict(Clock::time_point t) -> void { + if (t <= time_stamp) { + return; + } + if (initialized) { auto dt = rmcs::util::delta_time(t, time_stamp); if (dt > config.reset_interval) { diff --git a/src/module/predictor/regular/robot_state.cpp b/src/module/predictor/regular/robot_state.cpp index e23a42a8..e7cd10d8 100644 --- a/src/module/predictor/regular/robot_state.cpp +++ b/src/module/predictor/regular/robot_state.cpp @@ -31,6 +31,10 @@ struct RegularRobotState::Impl { } auto predict(Clock::time_point t) -> void { + if (t <= time_stamp) { + return; + } + if (initialized) { auto dt = util::delta_time(t, time_stamp); if (dt > reset_interval) { diff --git a/test/timestamp_alignment.cpp b/test/timestamp_alignment.cpp index 0ff7fdde..149da0e9 100644 --- a/test/timestamp_alignment.cpp +++ b/test/timestamp_alignment.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include #include @@ -133,6 +134,56 @@ TEST(TimestampAlignment, HistoryClientFindLatestChoosesNewestStateNotAfterImageT recv.find_latest([&](const ControlState& state) { return state.timestamp < base; }, matched)); } +TEST(TimestampAlignment, LateTriggerEventMustNotBindToNextFrame) { + using Channel = HistoryClient; + + auto shm_name = ShmScope { unique_shm_name("rmcs_auto_aim_trigger_late") }; + auto send = Channel::Send {}; + auto recv = Channel::Recv {}; + + ASSERT_TRUE(send.open(shm_name.c_str())); + ASSERT_TRUE(recv.open(shm_name.c_str())); + + auto last_bound_trigger_seq = std::uint64_t { 0 }; + auto last_image_capture_timestamp = std::optional {}; + constexpr auto trigger_sync_max_age = 50ms; + + auto bind = [&](Clock::time_point capture_timestamp) -> std::optional { + auto trigger = CameraTriggerEvent {}; + auto ok = recv.find_latest( + [&](const 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; + }, + trigger); + + if (ok) { + last_bound_trigger_seq = trigger.seq; + last_image_capture_timestamp = capture_timestamp; + return trigger; + } + + last_image_capture_timestamp = capture_timestamp; + return std::nullopt; + }; + + auto base = Clock::now(); + + EXPECT_FALSE(bind(base + 15ms).has_value()); + + ASSERT_TRUE(send.push(CameraTriggerEvent { .seq = 100, .timestamp = base + 10ms })); + EXPECT_FALSE(bind(base + 30ms).has_value()); + + ASSERT_TRUE(send.push(CameraTriggerEvent { .seq = 101, .timestamp = base + 38ms })); + auto matched = bind(base + 40ms); + ASSERT_TRUE(matched.has_value()); + EXPECT_EQ(matched->seq, 101U); + EXPECT_EQ(matched->timestamp, base + 38ms); +} + TEST(TimestampAlignment, FeishuFetchLatestBeforeUsesControlStateHistorySemantics) { auto shm_scope = FeishuShmScope {}; auto control = Feishu {}; From b32a55402bed89c52edf425d11358eee6df31ee4 Mon Sep 17 00:00:00 2001 From: heyeuu <2829004293@qq.com> Date: Thu, 23 Apr 2026 20:33:56 +0800 Subject: [PATCH 5/5] fix: fix YAML configuration parsing --- src/kernel/capturer.cpp | 9 ++++----- test/timestamp_alignment.cpp | 9 +++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/kernel/capturer.cpp b/src/kernel/capturer.cpp index 86c53615..7545d6e7 100644 --- a/src/kernel/capturer.cpp +++ b/src/kernel/capturer.cpp @@ -67,8 +67,7 @@ struct Capturer::Impl { return std::unexpected { instantitation_result.error() }; } - auto trigger_sync_config = yaml["enable_trigger_sync"].as(); - enable_trigger_sync = (source == "hikcamera" && trigger_sync_config); + enable_trigger_sync = source == "hikcamera" && yaml["enable_trigger_sync"].as(); auto show_loss_framerate = yaml["show_loss_framerate"].as(); auto show_loss_framerate_interval = yaml["show_loss_framerate_interval"].as(); @@ -106,9 +105,9 @@ struct Capturer::Impl { log.info("[Capturer runtime thread] starts"); // Success context - auto missing_trigger_limit = util::TimesLimit { 3 }; + auto missing_trigger_limit = util::TimesLimit { 3 }; auto last_image_capture_timestamp = std::optional {}; - auto bind_trigger_timestamp = [&](std::unique_ptr& image) { + auto bind_trigger_timestamp = [&](std::unique_ptr& image) { if (!enable_trigger_sync) { return; } @@ -123,7 +122,7 @@ struct Capturer::Impl { && capture_timestamp - candidate.timestamp <= trigger_sync_max_age; })) { image->set_timestamp(trigger->timestamp); - last_bound_trigger_seq_ = trigger->seq; + last_bound_trigger_seq_ = trigger->seq; last_image_capture_timestamp = capture_timestamp; missing_trigger_limit.reset(); missing_trigger_limit.enable(); diff --git a/test/timestamp_alignment.cpp b/test/timestamp_alignment.cpp index 149da0e9..fa1bd699 100644 --- a/test/timestamp_alignment.cpp +++ b/test/timestamp_alignment.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include "kernel/feishu.hpp" #include "utility/shared/interprocess.hpp" @@ -130,8 +131,8 @@ TEST(TimestampAlignment, HistoryClientFindLatestChoosesNewestStateNotAfterImageT [&](const ControlState& state) { return state.timestamp <= base + 10ms; }, matched)); EXPECT_EQ(matched.timestamp, base + 10ms); - EXPECT_FALSE( - recv.find_latest([&](const ControlState& state) { return state.timestamp < base; }, matched)); + EXPECT_FALSE(recv.find_latest( + [&](const ControlState& state) { return state.timestamp < base; }, matched)); } TEST(TimestampAlignment, LateTriggerEventMustNotBindToNextFrame) { @@ -144,8 +145,8 @@ TEST(TimestampAlignment, LateTriggerEventMustNotBindToNextFrame) { ASSERT_TRUE(send.open(shm_name.c_str())); ASSERT_TRUE(recv.open(shm_name.c_str())); - auto last_bound_trigger_seq = std::uint64_t { 0 }; - auto last_image_capture_timestamp = std::optional {}; + auto last_bound_trigger_seq = std::uint64_t { 0 }; + auto last_image_capture_timestamp = std::optional {}; constexpr auto trigger_sync_max_age = 50ms; auto bind = [&](Clock::time_point capture_timestamp) -> std::optional {