awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update GPR and SAR3D processing

Browse Source
This commit is contained in:
Ayzen
2026-10-08 16:24:01 +03:00
parent 390015ec39
commit 3575ca68f5
67 changed files with 1187 additions and 2581 deletions
Download Patch File Download Diff File Expand all files Collapse all files
data_acq_and_processing/processing/locator/include/locator/payload_builder.hpp
+3 -42
View File
@@ -1,7 +1,6 @@
#pragma once
#include <cstdint>
#include <optional>
#include <string>
#include <vector>
@@ -10,47 +9,9 @@
namespace radar::locator {
// Inclusive bounds used to clip locator observations to a visible window.
struct VisibleBounds {
float x_min = 0.0F;
float x_max = 0.0F;
float z_min = 0.0F;
float z_max = 0.0F;
};
// Limits mirroring the GUI semantics: if the number of detected objects exceeds
// `max_detected_objects`, the result is intentionally empty (matches Python
// reference). Otherwise, at most `draw_top_objects` rows are emitted.
struct DrawLimits {
std::uint32_t max_detected_objects = 0;
std::uint32_t draw_top_objects = 0;
};
// Filter parameters for the payload builder. None of these fields couple to GUI
// state; they are resolved by the data_processor from the live processing config
// and passed in explicitly so the builder remains a pure function.
struct FilterParams {
float min_score = 0.0F;
std::optional<VisibleBounds> visible_bounds{};
std::optional<DrawLimits> draw_limits{};
};
// Extract observations from one result collection.
//
// Mirrors `python_app/orchestration/gpr_locator.py::locator_observations_from_collection`:
// * Looks for a `gpr_points` TableF32 payload first, then falls back to
// `gpr_region_centers`. Both layouts encode `[x_m, z_m, score, ...]` rows.
// * Drops rows with non-finite coordinates and those below `min_score`.
// * If `visible_bounds` are provided, drops rows outside the inclusive window.
// * If `draw_limits` are provided and the surviving count exceeds
// `max_detected_objects`, returns an empty vector (the GUI's "too many to
// trust" heuristic). Otherwise, keeps the first `draw_top_objects` rows.
//
// The function never throws; malformed payloads degrade to an empty result.
[[nodiscard]] auto observations_from_collection(
const ipc::ResultCollection& collection,
const FilterParams& filter
) -> std::vector<Observation>;
// Extract the final coherent-GPR objects, dropping malformed or non-finite rows.
[[nodiscard]] auto observations_from_collection(const ipc::ResultCollection& collection)
-> std::vector<Observation>;
// Serialize a list of observations into the JSON payload format expected by
// locator clients: `{"ver": <n>, "tim": "HH:MM:SS.mmm", "sts": 1, "obs": [...]}`.
data_acq_and_processing/processing/locator/include/locator/tcp_server.hpp
+1 -1
View File
@@ -138,7 +138,7 @@ class TcpServer {
// Build and broadcast one locator packet derived from a GPR result
// collection. Never blocks for I/O. Filter parameters are supplied by the
// caller so the builder remains state-free.
void publish(const ipc::ResultCollection& collection, const FilterParams& filter);
void publish(const ipc::ResultCollection& collection);
// Most recent `vlc` value received from any connected client, or nullopt
// if no client has ever submitted one since startup. Lock-free.
data_acq_and_processing/processing/locator/src/payload_builder.cpp
+10 -84
View File
@@ -19,14 +19,12 @@ namespace {
using Json = nlohmann::json;
constexpr const char* kGprPointsName = "gpr_points";
constexpr const char* kGprRegionCentersName = "gpr_region_centers";
constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
// Locate the first TableF32 payload matching one of the GPR object names.
// Returns nullptr if no usable payload is present in the collection.
[[nodiscard]] auto find_object_table(const ipc::ResultCollection& collection)
-> const ipc::ResultPayload* {
const ipc::ResultPayload* fallback = nullptr;
for (const auto& payload : collection.collection_payloads) {
if (payload.kind != ipc::ResultKind::TableF32) {
continue;
@@ -37,11 +35,8 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
if (payload.processing_name == kGprPointsName) {
return &payload;
}
if (fallback == nullptr && payload.processing_name == kGprRegionCentersName) {
fallback = &payload;
}
}
return fallback;
return nullptr;
}
// Quantise to one decimal place (0.1 m = decimetres). Equivalent to Python's
@@ -51,29 +46,6 @@ constexpr std::uint32_t kMinTableColumns = 3U; // [x_m, z_m, score, ...]
return std::round(value * 10.0) / 10.0;
}
[[nodiscard]] auto passes_basic_filter(
float x_m,
float z_m,
float score,
float min_score
) -> bool {
if (!std::isfinite(x_m) || !std::isfinite(z_m) || !std::isfinite(score)) {
return false;
}
return score >= min_score;
}
[[nodiscard]] auto passes_visible_bounds(
float x_m,
float z_m,
const VisibleBounds& bounds
) -> bool {
return x_m >= bounds.x_min
&& x_m <= bounds.x_max
&& z_m >= bounds.z_min
&& z_m <= bounds.z_max;
}
// Render the current wall-clock time as "HH:MM:SS.mmm". Uses localtime to match
// the Python reference behaviour (which calls datetime.now() with no tz info).
[[nodiscard]] auto format_timestamp_now() -> std::string {
@@ -107,66 +79,20 @@ void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t v
} // namespace
auto observations_from_collection(
const ipc::ResultCollection& collection,
const FilterParams& filter
) -> std::vector<Observation> {
auto observations_from_collection(const ipc::ResultCollection& collection) -> std::vector<Observation> {
const auto* payload = find_object_table(collection);
if (payload == nullptr) {
return {};
}
const std::uint32_t columns = payload->table_columns;
if (columns == 0U) {
return {};
}
const std::size_t row_count = payload->table_values.size() / columns;
if (row_count == 0U) {
return {};
}
std::vector<Observation> visible;
visible.reserve(row_count);
for (std::size_t row = 0; row < row_count; ++row) {
const std::size_t base = row * static_cast<std::size_t>(columns);
const float x_m = payload->table_values[base + 0U];
const float z_m = payload->table_values[base + 1U];
const float score = payload->table_values[base + 2U];
if (!passes_basic_filter(x_m, z_m, score, filter.min_score)) {
continue;
}
if (filter.visible_bounds.has_value()
&& !passes_visible_bounds(x_m, z_m, *filter.visible_bounds)) {
continue;
}
visible.push_back({.crs = x_m, .dst = z_m});
}
if (!filter.draw_limits.has_value()) {
std::vector<Observation> quantised;
quantised.reserve(visible.size());
for (const auto& observation : visible) {
quantised.push_back({
.crs = quantise_to_decimetres(observation.crs),
.dst = quantise_to_decimetres(observation.dst),
});
}
return quantised;
}
const auto& limits = *filter.draw_limits;
if (visible.size() > limits.max_detected_objects) {
return {};
}
const std::size_t kept = std::min<std::size_t>(visible.size(), limits.draw_top_objects);
const std::size_t columns = payload->table_columns;
std::vector<Observation> result;
result.reserve(kept);
for (std::size_t index = 0; index < kept; ++index) {
result.push_back({
.crs = quantise_to_decimetres(visible[index].crs),
.dst = quantise_to_decimetres(visible[index].dst),
});
for (std::size_t base = 0; base + columns <= payload->table_values.size(); base += columns) {
const auto x = payload->table_values[base];
const auto z = payload->table_values[base + 1];
const auto score = payload->table_values[base + 2];
if (std::isfinite(x) && std::isfinite(z) && std::isfinite(score)) {
result.push_back({.crs = quantise_to_decimetres(x), .dst = quantise_to_decimetres(z)});
}
}
return result;
}
data_acq_and_processing/processing/locator/src/tcp_server.cpp
+2 -2
View File
@@ -461,12 +461,12 @@ auto TcpServer::is_running() const -> bool {
return running_.load(std::memory_order_acquire);
}
void TcpServer::publish(const ipc::ResultCollection& collection, const FilterParams& filter) {
void TcpServer::publish(const ipc::ResultCollection& collection) {
if (!running_.load(std::memory_order_acquire)) {
return;
}
const auto observations = observations_from_collection(collection, filter);
const auto observations = observations_from_collection(collection);
const auto payload_json = build_payload_json(observations, config_.protocol_version);
auto packet = encode_packet(payload_json, config_.device_id);
cache_latest_packet(packet);