Skip to content

feat: update decider logic #52

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
creeper5820 merged 1 commit into from
May 20, 2026
Merged

feat: update decider logic #52

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
53 changes: 37 additions & 16 deletions src/module/tracker/decider.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
#include <limits>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>

using namespace rmcs::tracker;
Expand All @@ -16,6 +15,7 @@ using namespace std::chrono_literals;
struct Decider::Impl {
static constexpr auto kDefaultCleanupInterval = 1.0s;
static constexpr auto kOutpostCleanupInterval = 1.5s;
static constexpr auto kLockLostTimeout = 150ms;

static constexpr auto get_cleanup_interval(DeviceId device_id) {
switch (device_id) {
Expand All @@ -31,7 +31,7 @@ struct Decider::Impl {

if (priority_mode.empty()) priority_mode = mode2;

return { };
return {};
}

auto set_priority_mode(PriorityMode const& mode) -> void { priority_mode = mode; }
Expand All @@ -42,8 +42,8 @@ struct Decider::Impl {
tracker->predict(t);
}

auto observed_ids = std::unordered_set<DeviceId> { };
auto grouped_armors = std::unordered_map<DeviceId, std::vector<Armor3D>> { };
auto fused_ids = DeviceIds::None();
auto grouped_armors = std::unordered_map<DeviceId, std::vector<Armor3D>> {};

for (const auto& armor : armors) {
grouped_armors[armor.genre].emplace_back(armor);
Expand All @@ -60,15 +60,15 @@ struct Decider::Impl {
bool fused = trackers[id]->update(grouped_span);

if (fused) {
observed_ids.insert(id);
fused_ids.append(id);
last_seen_times[id] = t;
}
}

std::erase_if(trackers, [&](const auto& item) {
auto last_seen_it = last_seen_times.find(item.first);
auto cleanup_interval = get_cleanup_interval(item.first);
bool expired = last_seen_it == last_seen_times.end()
bool expired = last_seen_it == last_seen_times.end()
|| util::delta_time(t, last_seen_it->second) > cleanup_interval;
if (expired) {
last_seen_times.erase(item.first);
Expand All @@ -77,9 +77,29 @@ struct Decider::Impl {
return expired;
});

auto fresh_target_id = arbitrate(observed_ids);
// 优先保持已锁定且仍在短时失观测宽限期内的目标
if (locked_target_id != DeviceId::UNKNOWN) {
auto locked_seen_it = last_seen_times.find(locked_target_id);
bool lock_alive = trackers.contains(locked_target_id)
&& locked_seen_it != last_seen_times.end()
&& util::delta_time(t, locked_seen_it->second) <= kLockLostTimeout;

// 宽限期内即使本帧没观测到，也继续保持 target_id；
// 但 make_output 会把 allow_control 置为 false。
if (lock_alive)
return make_output(locked_target_id, fused_ids.contains(locked_target_id));

locked_target_id = DeviceId::UNKNOWN;
}

auto fresh_target_id = arbitrate(fused_ids);
if (fresh_target_id != DeviceId::UNKNOWN) {
return make_output(fresh_target_id, trackers.at(fresh_target_id)->is_converged());
bool allow_control = trackers.at(fresh_target_id)->is_converged();

// 只有收敛后的目标才会进入锁定；未收敛目标可以输出，但不会写成锁定目标。
if (allow_control) locked_target_id = fresh_target_id;

return make_output(fresh_target_id, true);
}

return Output {
Expand All @@ -89,11 +109,11 @@ struct Decider::Impl {
};
}

auto arbitrate(const std::unordered_set<DeviceId>& observed_ids) -> DeviceId {
auto arbitrate(DeviceIds fused_ids) -> DeviceId {
auto best_target_id = DeviceId::UNKNOWN;

for (const auto& [device_id, _] : trackers) {
if (!observed_ids.contains(device_id)) continue;
if (!fused_ids.contains(device_id)) continue;

if (best_target_id == DeviceId::UNKNOWN
|| is_better_target(device_id, best_target_id)) {
Expand All @@ -104,11 +124,11 @@ struct Decider::Impl {
return best_target_id;
}

auto make_output(DeviceId device_id, bool allow_takeover) const -> Output {
auto make_output(DeviceId device_id, bool fused) const -> Output {
return Output {
.target_id = device_id,
.snapshot = trackers.at(device_id)->get_snapshot(),
.allow_control = allow_takeover,
.allow_control = fused && trackers.at(device_id)->is_converged(),
};
}

Expand All @@ -126,11 +146,11 @@ struct Decider::Impl {
auto safe_distance =
std::isfinite(distance) ? distance : std::numeric_limits<double>::infinity();

// 比较顺序：优先级 -> 收敛状态 -> 距离 -> 固定 ID 兜底。
// 比较顺序：距离 -> 收敛状态 -> 优先级 -> 固定 ID 兜底。
return std::tuple {
priority_of(device_id), // 数值越小，优先级越高。
!tracker.is_converged(), // 收敛目标映射为 0，未收敛目标映射为 1。
safe_distance, // 非有限距离按无穷远处理，避免 NaN/Inf 干扰排序。
!tracker.is_converged(), // 收敛目标映射为 0，未收敛目标映射为 1。
priority_of(device_id), // 数值越小，优先级越高。
rmcs::to_index(device_id), // 完全相同时按固定顺序兜底，避免容器遍历顺序抖动。
};
};
Expand All @@ -140,6 +160,7 @@ struct Decider::Impl {

std::unordered_map<DeviceId, std::unique_ptr<RobotState>> trackers;
std::unordered_map<DeviceId, TimePoint> last_seen_times;
DeviceId locked_target_id { DeviceId::UNKNOWN };

PriorityMode priority_mode;

Expand All @@ -163,7 +184,7 @@ struct Decider::Impl {
{ DeviceId::INFANTRY_5, 5 },
{ DeviceId::SENTRY, 3 },
{ DeviceId::OUTPOST, 1 },
{ DeviceId::BASE, 5 },
{ DeviceId::BASE, 4 },
{ DeviceId::UNKNOWN, 5 },
};
};
Expand Down
Loading