added timing
This commit is contained in:
Ayzen
@@ -4,6 +4,7 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "locator/locator_config.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::config {
|
||||
@@ -148,6 +149,7 @@ struct RunConfig {
|
||||
RuntimeConfig runtime{};
|
||||
PreprocessConfig preprocess{};
|
||||
GprConfig gpr{};
|
||||
radar::locator::LocatorServerConfig locator_server{};
|
||||
std::vector<radar::ipc::ComboKey> run_combos{};
|
||||
};
|
||||
|
||||
|
||||
@@ -194,6 +194,33 @@ using Json = nlohmann::json;
|
||||
return notch;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_locator_server(const Json& object) -> radar::locator::LocatorServerConfig {
|
||||
radar::locator::LocatorServerConfig locator{};
|
||||
locator.enabled = optional_bool(object, "enabled", locator.enabled);
|
||||
locator.host = optional_string(object, "host", locator.host);
|
||||
locator.device_id = optional_u32(object, "device_id", locator.device_id);
|
||||
locator.protocol_version = optional_u32(object, "protocol_version", locator.protocol_version);
|
||||
locator.max_payload_bytes = optional_u32(object, "max_payload_bytes", locator.max_payload_bytes);
|
||||
locator.client_queue_size = optional_u32(object, "client_queue_size", locator.client_queue_size);
|
||||
|
||||
if (const auto* port_value = optional_field(object, "port"); port_value != nullptr) {
|
||||
const auto port_u32 = number_to_u32(as_number(*port_value, "run.locator_server.port"),
|
||||
"run.locator_server.port");
|
||||
if (port_u32 == 0U || port_u32 > 0xFFFFU) {
|
||||
throw std::runtime_error("run.locator_server.port must be in [1, 65535]");
|
||||
}
|
||||
locator.port = static_cast<std::uint16_t>(port_u32);
|
||||
}
|
||||
|
||||
if (locator.client_queue_size == 0U) {
|
||||
throw std::runtime_error("run.locator_server.client_queue_size must be > 0");
|
||||
}
|
||||
if (locator.max_payload_bytes == 0U) {
|
||||
throw std::runtime_error("run.locator_server.max_payload_bytes must be > 0");
|
||||
}
|
||||
return locator;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto parse_driver_mode(const std::string& value) -> DriverMode {
|
||||
if (value == "mock") {
|
||||
return DriverMode::Mock;
|
||||
@@ -479,6 +506,12 @@ auto load_run_config(const std::string& path) -> RunConfig {
|
||||
"python_app/runtime/processing_live.json"
|
||||
);
|
||||
|
||||
if (const auto* locator_value = optional_field(*run_obj, "locator_server");
|
||||
locator_value != nullptr) {
|
||||
config.locator_server =
|
||||
parse_locator_server(*as_object(*locator_value, "run.locator_server"));
|
||||
}
|
||||
|
||||
const auto* combos = as_array(required_field(*run_obj, "combos"), "run.combos");
|
||||
if (combos->empty()) {
|
||||
throw std::runtime_error("run.combos must not be empty");
|
||||
|
||||
@@ -83,6 +83,11 @@ struct ResultBlock {
|
||||
struct ResultCollection {
|
||||
std::uint64_t collection_id = 0;
|
||||
std::uint64_t monotonic_ns = 0;
|
||||
// Wall-clock duration of `process_collection()` for this collection,
|
||||
// measured on the data_processor side with a monotonic clock. Used by the
|
||||
// Python pipeline to report processing-time metrics. Zero is a valid
|
||||
// "unmeasured" sentinel for legacy producers.
|
||||
std::uint64_t processing_duration_ns = 0;
|
||||
std::vector<ResultPayload> collection_payloads{};
|
||||
std::vector<ResultBlock> blocks{};
|
||||
};
|
||||
|
||||
@@ -14,7 +14,7 @@ namespace {
|
||||
|
||||
constexpr std::uint32_t kRawCollectionMagic = 0x32574152U; // RAW2
|
||||
constexpr std::uint32_t kPreprocessedCollectionMagic = 0x32525050U; // PRP2
|
||||
constexpr std::uint32_t kResultCollectionMagic = 0x314C5352U; // RSL1
|
||||
constexpr std::uint32_t kResultCollectionMagic = 0x324C5352U; // RSL2
|
||||
|
||||
template <typename T>
|
||||
concept TriviallySerializable = std::is_trivially_copyable_v<T>;
|
||||
@@ -408,6 +408,7 @@ auto serialize_result_collection(const ResultCollection& collection) -> std::vec
|
||||
writer.write(kResultCollectionMagic);
|
||||
writer.write(collection.collection_id);
|
||||
writer.write(collection.monotonic_ns);
|
||||
writer.write(collection.processing_duration_ns);
|
||||
writer.write(checked_count_to_u32(collection.collection_payloads.size(), "Collection payload count"));
|
||||
writer.write(checked_count_to_u32(collection.blocks.size(), "Result block count"));
|
||||
|
||||
@@ -432,6 +433,7 @@ auto deserialize_result_collection(std::span<const std::uint8_t> bytes) -> Resul
|
||||
ResultCollection collection{};
|
||||
collection.collection_id = reader.read<std::uint64_t>();
|
||||
collection.monotonic_ns = reader.read<std::uint64_t>();
|
||||
collection.processing_duration_ns = reader.read<std::uint64_t>();
|
||||
|
||||
const auto collection_payload_count = reader.read<std::uint32_t>();
|
||||
const auto block_count = reader.read<std::uint32_t>();
|
||||
|
||||
@@ -7,6 +7,7 @@
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "locator/tcp_server.hpp"
|
||||
#include "processor_interface.hpp"
|
||||
#include "processing_live_config.hpp"
|
||||
#include "run_config.hpp"
|
||||
@@ -22,7 +23,8 @@ class DataProcessor {
|
||||
const config::RunConfig& config,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing& results_ring,
|
||||
ProcessorRegistry processors
|
||||
ProcessorRegistry processors,
|
||||
radar::locator::TcpServer* locator_server = nullptr
|
||||
);
|
||||
|
||||
void run(const std::atomic<bool>& stop_requested);
|
||||
@@ -38,12 +40,26 @@ class DataProcessor {
|
||||
[[nodiscard]] auto resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface&;
|
||||
[[nodiscard]] auto should_replay_entire_history(const ProcessingLiveConfig& live_config) const -> bool;
|
||||
|
||||
// Merge live config with the latest socket-supplied speed (if any and if
|
||||
// not suppressed by `ignore_socket_speed`). The returned config is what
|
||||
// actually drives processing for this tick.
|
||||
[[nodiscard]] auto resolve_effective_live_config(const ProcessingLiveConfig& live_config) const
|
||||
-> ProcessingLiveConfig;
|
||||
|
||||
// Convert one processed collection into a locator filter spec, derived
|
||||
// from the live config and current processor mode.
|
||||
[[nodiscard]] auto build_locator_filter(const ProcessingLiveConfig& live_config) const
|
||||
-> radar::locator::FilterParams;
|
||||
|
||||
void publish_locator(const ipc::ResultCollection& collection, const ProcessingLiveConfig& live_config);
|
||||
|
||||
const config::RunConfig& config_;
|
||||
ipc::ShmRing& preprocessed_ring_;
|
||||
ipc::ShmRing& results_ring_;
|
||||
ProcessorRegistry processors_{};
|
||||
std::string default_processor_mode_{};
|
||||
ProcessingLiveConfigLoader live_config_loader_;
|
||||
radar::locator::TcpServer* locator_server_ = nullptr;
|
||||
};
|
||||
|
||||
[[nodiscard]] auto create_default_processors() -> ProcessorRegistry;
|
||||
|
||||
@@ -52,6 +52,17 @@ struct ProcessingLiveConfig {
|
||||
// BP image is computed in the y=imaging_plane_y_m slice of the 3D grid.
|
||||
// Default 0 keeps legacy 1D antenna layouts imaging in the antenna plane.
|
||||
float gpr_imaging_plane_y_m = 0.0F;
|
||||
// Locator filter parameters. Mode-dependent threshold (legacy_gpr uses
|
||||
// `legacy_gpr_min_visible_pair_count`, everything else uses
|
||||
// `gpr_min_visible_score`). Draw limits apply only to non-legacy modes.
|
||||
float gpr_min_visible_score = 0.0F;
|
||||
float legacy_gpr_min_visible_pair_count = 0.0F;
|
||||
std::uint32_t gpr_max_detected_objects_to_draw = 0;
|
||||
std::uint32_t gpr_draw_top_m_objects = 0;
|
||||
// When true, the data_processor ignores socket-supplied `vlc` updates and
|
||||
// keeps using `gpr_speed_m_s` from this file. Mirrored from the GUI's
|
||||
// "ignore socket speed" checkbox.
|
||||
bool ignore_socket_speed = false;
|
||||
bool reprocess_current_result = true;
|
||||
std::uint64_t history_command_seq = 0;
|
||||
HistoryCommand history_command = HistoryCommand::None;
|
||||
|
||||
@@ -37,14 +37,16 @@ DataProcessor::DataProcessor(
|
||||
const config::RunConfig& config,
|
||||
ipc::ShmRing& preprocessed_ring,
|
||||
ipc::ShmRing& results_ring,
|
||||
ProcessorRegistry processors
|
||||
ProcessorRegistry processors,
|
||||
radar::locator::TcpServer* locator_server
|
||||
)
|
||||
: config_(config),
|
||||
preprocessed_ring_(preprocessed_ring),
|
||||
results_ring_(results_ring),
|
||||
processors_(std::move(processors)),
|
||||
default_processor_mode_(kDefaultProcessorMode),
|
||||
live_config_loader_(config.runtime.processing_live_config_path) {
|
||||
live_config_loader_(config.runtime.processing_live_config_path),
|
||||
locator_server_(locator_server) {
|
||||
if (processors_.empty()) {
|
||||
throw std::runtime_error("DataProcessor requires at least one processor");
|
||||
}
|
||||
@@ -61,7 +63,8 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
std::uint64_t last_applied_history_command_seq = 0;
|
||||
|
||||
while (!stop_requested.load(std::memory_order_relaxed)) {
|
||||
const auto live_config = live_config_loader_.refresh_if_needed();
|
||||
const auto live_config_raw = live_config_loader_.refresh_if_needed();
|
||||
const auto live_config = resolve_effective_live_config(live_config_raw);
|
||||
const auto live_revision = live_config_loader_.revision();
|
||||
auto& processor = resolve_processor(live_config);
|
||||
|
||||
@@ -89,6 +92,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
publish_locator(replay_result, live_config);
|
||||
}
|
||||
} else if (!preprocessed_history.empty()) {
|
||||
const auto replay_result = process_collection(
|
||||
@@ -98,6 +102,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(replay_result, results_ring_);
|
||||
publish_locator(replay_result, live_config);
|
||||
}
|
||||
last_replayed_revision = live_revision;
|
||||
}
|
||||
@@ -116,6 +121,7 @@ void DataProcessor::run(const std::atomic<bool>& stop_requested) {
|
||||
live_config
|
||||
);
|
||||
publish_result_collection(result_collection, results_ring_);
|
||||
publish_locator(result_collection, live_config);
|
||||
continue;
|
||||
}
|
||||
|
||||
@@ -129,7 +135,13 @@ auto DataProcessor::process_collection(
|
||||
ProcessorInterface& processor,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) -> ipc::ResultCollection {
|
||||
return processor.process_collection(config_, preprocessed, previous_collections, live_config);
|
||||
const auto started_at = std::chrono::steady_clock::now();
|
||||
auto result = processor.process_collection(config_, preprocessed, previous_collections, live_config);
|
||||
const auto finished_at = std::chrono::steady_clock::now();
|
||||
result.processing_duration_ns = static_cast<std::uint64_t>(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(finished_at - started_at).count()
|
||||
);
|
||||
return result;
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_processor(const ProcessingLiveConfig& live_config) -> ProcessorInterface& {
|
||||
@@ -151,6 +163,54 @@ auto DataProcessor::should_replay_entire_history(const ProcessingLiveConfig& liv
|
||||
return requested_mode == "bscan";
|
||||
}
|
||||
|
||||
auto DataProcessor::resolve_effective_live_config(const ProcessingLiveConfig& live_config) const
|
||||
-> ProcessingLiveConfig {
|
||||
if (live_config.ignore_socket_speed || locator_server_ == nullptr) {
|
||||
return live_config;
|
||||
}
|
||||
const auto socket_speed = locator_server_->latest_socket_speed();
|
||||
if (!socket_speed.has_value()) {
|
||||
return live_config;
|
||||
}
|
||||
ProcessingLiveConfig effective = live_config;
|
||||
effective.gpr_speed_m_s = static_cast<float>(*socket_speed);
|
||||
return effective;
|
||||
}
|
||||
|
||||
auto DataProcessor::build_locator_filter(const ProcessingLiveConfig& live_config) const
|
||||
-> radar::locator::FilterParams {
|
||||
const std::string requested_mode =
|
||||
live_config.processor_mode.empty() ? default_processor_mode_ : live_config.processor_mode;
|
||||
|
||||
radar::locator::FilterParams filter{};
|
||||
if (requested_mode == "legacy_gpr") {
|
||||
filter.min_score = live_config.legacy_gpr_min_visible_pair_count;
|
||||
// The GUI deliberately disables the "draw top N" capping for legacy
|
||||
// GPR, so we also skip it on the wire to match observation semantics.
|
||||
filter.draw_limits.reset();
|
||||
} else {
|
||||
filter.min_score = live_config.gpr_min_visible_score;
|
||||
if (live_config.gpr_max_detected_objects_to_draw > 0U
|
||||
&& live_config.gpr_draw_top_m_objects > 0U) {
|
||||
filter.draw_limits = radar::locator::DrawLimits{
|
||||
.max_detected_objects = live_config.gpr_max_detected_objects_to_draw,
|
||||
.draw_top_objects = live_config.gpr_draw_top_m_objects,
|
||||
};
|
||||
}
|
||||
}
|
||||
return filter;
|
||||
}
|
||||
|
||||
void DataProcessor::publish_locator(
|
||||
const ipc::ResultCollection& collection,
|
||||
const ProcessingLiveConfig& live_config
|
||||
) {
|
||||
if (locator_server_ == nullptr || !locator_server_->is_running()) {
|
||||
return;
|
||||
}
|
||||
locator_server_->publish(collection, build_locator_filter(live_config));
|
||||
}
|
||||
|
||||
auto create_default_processors() -> ProcessorRegistry {
|
||||
ProcessorRegistry processors{};
|
||||
{
|
||||
|
||||
@@ -2,9 +2,11 @@
|
||||
#include <csignal>
|
||||
#include <exception>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "data_processor.hpp"
|
||||
#include "locator/tcp_server.hpp"
|
||||
#include "run_config.hpp"
|
||||
#include "shm_ring.hpp"
|
||||
|
||||
@@ -21,6 +23,8 @@ void signal_handler(int /*signal*/) {
|
||||
void install_signal_handlers() {
|
||||
std::signal(SIGINT, signal_handler);
|
||||
std::signal(SIGTERM, signal_handler);
|
||||
// Writing to a peer-closed socket would otherwise terminate the process.
|
||||
std::signal(SIGPIPE, SIG_IGN);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto read_config_path(int argc, char** argv) -> std::string {
|
||||
@@ -34,6 +38,21 @@ void install_signal_handlers() {
|
||||
return config_path;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto start_locator_server(const radar::config::RunConfig& config)
|
||||
-> std::unique_ptr<radar::locator::TcpServer> {
|
||||
if (!config.locator_server.enabled) {
|
||||
return nullptr;
|
||||
}
|
||||
auto server = std::make_unique<radar::locator::TcpServer>(config.locator_server);
|
||||
try {
|
||||
server->start();
|
||||
} catch (const std::exception& exception) {
|
||||
std::cerr << "data_processor: locator server disabled (" << exception.what() << ")\n";
|
||||
return nullptr;
|
||||
}
|
||||
return server;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
@@ -54,11 +73,14 @@ int main(int argc, char** argv) {
|
||||
config.rings.results.slot_size_bytes
|
||||
);
|
||||
|
||||
auto locator_server = start_locator_server(config);
|
||||
|
||||
radar::processing::DataProcessor processor(
|
||||
config,
|
||||
preprocessed_ring,
|
||||
results_ring,
|
||||
radar::processing::create_default_processors()
|
||||
radar::processing::create_default_processors(),
|
||||
locator_server.get()
|
||||
);
|
||||
processor.run(g_stop_requested);
|
||||
return 0;
|
||||
|
||||
@@ -311,6 +311,32 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
|
||||
}
|
||||
config.gpr_imaging_plane_y_m = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_min_visible_score"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.gpr_min_visible_score must be number");
|
||||
}
|
||||
config.gpr_min_visible_score = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("legacy_gpr_min_visible_pair_count"); found != root.end()) {
|
||||
if (!found->is_number()) {
|
||||
throw std::runtime_error("processing.legacy_gpr_min_visible_pair_count must be number");
|
||||
}
|
||||
config.legacy_gpr_min_visible_pair_count = static_cast<float>(found->get<double>());
|
||||
}
|
||||
if (const auto found = root.find("gpr_max_detected_objects_to_draw"); found != root.end()) {
|
||||
config.gpr_max_detected_objects_to_draw =
|
||||
parse_u32_number(*found, "processing.gpr_max_detected_objects_to_draw");
|
||||
}
|
||||
if (const auto found = root.find("gpr_draw_top_m_objects"); found != root.end()) {
|
||||
config.gpr_draw_top_m_objects =
|
||||
parse_u32_number(*found, "processing.gpr_draw_top_m_objects");
|
||||
}
|
||||
if (const auto found = root.find("ignore_socket_speed"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.ignore_socket_speed must be bool");
|
||||
}
|
||||
config.ignore_socket_speed = found->get<bool>();
|
||||
}
|
||||
if (const auto found = root.find("reprocess_current_result"); found != root.end()) {
|
||||
if (!found->is_boolean()) {
|
||||
throw std::runtime_error("processing.reprocess_current_result must be bool");
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
namespace radar::locator {
|
||||
|
||||
// Static configuration for the locator TCP server, mirrored from `run.locator_server`
|
||||
// in `run_config.json`. Values are validated by the config parser before reaching here.
|
||||
struct LocatorServerConfig {
|
||||
bool enabled = true;
|
||||
std::string host = "0.0.0.0";
|
||||
std::uint16_t port = 8888;
|
||||
std::uint32_t device_id = 3;
|
||||
std::uint32_t protocol_version = 1;
|
||||
std::uint32_t max_payload_bytes = 64U * 1024U;
|
||||
std::uint32_t client_queue_size = 32;
|
||||
};
|
||||
|
||||
} // namespace radar::locator
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
namespace radar::locator {
|
||||
|
||||
// One outbound locator observation: object position relative to the radar in metres.
|
||||
// `crs` is the cross-range (X) coordinate; `dst` is the range (Z) coordinate.
|
||||
// Values are kept as float because they are quantised to two decimal places before
|
||||
// being serialised on the wire.
|
||||
struct Observation {
|
||||
float crs = 0.0F;
|
||||
float dst = 0.0F;
|
||||
};
|
||||
|
||||
} // namespace radar::locator
|
||||
@@ -0,0 +1,72 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "locator/observation.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
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>;
|
||||
|
||||
// Serialize a list of observations into the JSON payload format expected by
|
||||
// locator clients: `{"ver": <n>, "tim": "HH:MM:SS.mmm", "sts": 1, "obs": [...]}`.
|
||||
// Each observation contributes `{"dst": <m>, "crs": <m>}` with two-decimal
|
||||
// quantisation.
|
||||
[[nodiscard]] auto build_payload_json(
|
||||
const std::vector<Observation>& observations,
|
||||
std::uint32_t protocol_version,
|
||||
std::uint32_t status = 1U
|
||||
) -> std::string;
|
||||
|
||||
// Wrap a JSON payload string into a framed wire packet:
|
||||
// `<device_id:u32 LE><payload_len:u32 LE><payload bytes...>`.
|
||||
[[nodiscard]] auto encode_packet(
|
||||
const std::string& payload_json,
|
||||
std::uint32_t device_id
|
||||
) -> std::vector<std::uint8_t>;
|
||||
|
||||
} // namespace radar::locator
|
||||
@@ -0,0 +1,165 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <deque>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include "locator/locator_config.hpp"
|
||||
#include "locator/payload_builder.hpp"
|
||||
#include "shared_types.hpp"
|
||||
|
||||
namespace radar::locator {
|
||||
|
||||
// Bounded, in-memory packet queue used by the per-client writer thread.
|
||||
// Marking the queue as full closes the client (back-pressure by disconnect),
|
||||
// matching the semantics of the previous Python implementation.
|
||||
class ClientQueue {
|
||||
public:
|
||||
explicit ClientQueue(std::size_t capacity);
|
||||
|
||||
// Push a packet onto the queue. Returns false if the queue is full or has
|
||||
// been closed; the caller is expected to disconnect the client in that case.
|
||||
[[nodiscard]] auto try_push(std::vector<std::uint8_t> packet) -> bool;
|
||||
|
||||
// Block until a packet is available or the queue is closed.
|
||||
// Returns nullopt iff the queue has been closed and is drained.
|
||||
[[nodiscard]] auto wait_pop() -> std::optional<std::vector<std::uint8_t>>;
|
||||
|
||||
// Wake any waiter and reject further pushes. Idempotent.
|
||||
void close();
|
||||
|
||||
[[nodiscard]] auto is_closed() const -> bool;
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_{};
|
||||
std::condition_variable not_empty_{};
|
||||
std::deque<std::vector<std::uint8_t>> queue_{};
|
||||
std::size_t capacity_;
|
||||
bool closed_ = false;
|
||||
};
|
||||
|
||||
// One connected locator client: owns its socket, writer thread, reader thread,
|
||||
// and outbound queue. Removed from the server's roster once both threads exit.
|
||||
class ClientSession {
|
||||
public:
|
||||
ClientSession(
|
||||
int socket_fd,
|
||||
std::string peer_name,
|
||||
std::size_t queue_capacity,
|
||||
std::uint32_t max_payload_bytes
|
||||
);
|
||||
|
||||
~ClientSession();
|
||||
|
||||
ClientSession(const ClientSession&) = delete;
|
||||
auto operator=(const ClientSession&) -> ClientSession& = delete;
|
||||
ClientSession(ClientSession&&) = delete;
|
||||
auto operator=(ClientSession&&) -> ClientSession& = delete;
|
||||
|
||||
// Start writer/reader threads. The shared `vlc` slot is notified whenever
|
||||
// an inbound packet carrying a finite `vlc` field arrives.
|
||||
void start(std::atomic<double>& shared_vlc_slot);
|
||||
|
||||
// Enqueue one outbound packet. Disconnects this session if the queue is
|
||||
// already full or the writer has stopped.
|
||||
void enqueue(std::vector<std::uint8_t> packet);
|
||||
|
||||
// Initiate teardown of this client (idempotent): closes the queue and
|
||||
// shuts the socket so writer/reader threads can exit promptly.
|
||||
void request_stop();
|
||||
|
||||
// Join writer/reader threads and release the socket. Must be called from a
|
||||
// thread other than this session's writer or reader.
|
||||
void join();
|
||||
|
||||
[[nodiscard]] auto has_exited() const -> bool;
|
||||
|
||||
[[nodiscard]] auto peer_name() const -> const std::string&;
|
||||
|
||||
private:
|
||||
void writer_loop();
|
||||
void reader_loop(std::atomic<double>& shared_vlc_slot);
|
||||
|
||||
int socket_fd_;
|
||||
std::string peer_name_;
|
||||
ClientQueue queue_;
|
||||
std::uint32_t max_payload_bytes_;
|
||||
std::atomic<bool> stop_requested_{false};
|
||||
std::atomic<bool> exited_{false};
|
||||
std::thread writer_thread_{};
|
||||
std::thread reader_thread_{};
|
||||
};
|
||||
|
||||
// Multi-client TCP locator server.
|
||||
//
|
||||
// Design contract:
|
||||
// * Threading: one acceptor thread + two threads per connected client. The
|
||||
// producer (data_processor) calls publish() synchronously; that call is
|
||||
// non-blocking and never throws for typical operation.
|
||||
// * Back-pressure: each client has its own bounded outbound queue. If a
|
||||
// client is too slow to drain, the next publish() drops it (matches the
|
||||
// prior Python service). Other clients are unaffected.
|
||||
// * Latest-snapshot: the most recently published packet is cached and sent
|
||||
// to every newly connected client before any new packets are forwarded.
|
||||
// * Lifetime: `start()` may throw on listen failure. `stop()` is idempotent
|
||||
// and is also invoked from the destructor.
|
||||
class TcpServer {
|
||||
public:
|
||||
explicit TcpServer(LocatorServerConfig config);
|
||||
|
||||
~TcpServer();
|
||||
|
||||
TcpServer(const TcpServer&) = delete;
|
||||
auto operator=(const TcpServer&) -> TcpServer& = delete;
|
||||
TcpServer(TcpServer&&) = delete;
|
||||
auto operator=(TcpServer&&) -> TcpServer& = delete;
|
||||
|
||||
// Bind the listening socket and start the acceptor thread.
|
||||
// Throws std::runtime_error on socket(), bind() or listen() failure.
|
||||
void start();
|
||||
|
||||
// Tear down all clients and the acceptor. Idempotent.
|
||||
void stop();
|
||||
|
||||
[[nodiscard]] auto is_running() const -> bool;
|
||||
|
||||
// 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);
|
||||
|
||||
// Most recent `vlc` value received from any connected client, or nullopt
|
||||
// if no client has ever submitted one since startup. Lock-free.
|
||||
[[nodiscard]] auto latest_socket_speed() const -> std::optional<double>;
|
||||
|
||||
private:
|
||||
void acceptor_loop();
|
||||
void enroll_client(std::unique_ptr<ClientSession> session);
|
||||
void broadcast_packet(const std::vector<std::uint8_t>& packet);
|
||||
void reap_finished_clients();
|
||||
void cache_latest_packet(std::vector<std::uint8_t> packet);
|
||||
[[nodiscard]] auto latest_packet_copy() const -> std::optional<std::vector<std::uint8_t>>;
|
||||
|
||||
LocatorServerConfig config_;
|
||||
std::atomic<bool> running_{false};
|
||||
int listen_fd_ = -1;
|
||||
std::thread acceptor_thread_{};
|
||||
|
||||
mutable std::mutex clients_mutex_{};
|
||||
std::vector<std::unique_ptr<ClientSession>> clients_{};
|
||||
|
||||
mutable std::mutex latest_packet_mutex_{};
|
||||
std::optional<std::vector<std::uint8_t>> latest_packet_{};
|
||||
|
||||
// Sentinel of "no value yet" is NaN. Lock-free read from data_processor.
|
||||
std::atomic<double> latest_socket_speed_{};
|
||||
};
|
||||
|
||||
} // namespace radar::locator
|
||||
@@ -0,0 +1,208 @@
|
||||
#include "locator/payload_builder.hpp"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace radar::locator {
|
||||
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;
|
||||
}
|
||||
if (payload.table_columns < kMinTableColumns) {
|
||||
continue;
|
||||
}
|
||||
if (payload.processing_name == kGprPointsName) {
|
||||
return &payload;
|
||||
}
|
||||
if (fallback == nullptr && payload.processing_name == kGprRegionCentersName) {
|
||||
fallback = &payload;
|
||||
}
|
||||
}
|
||||
return fallback;
|
||||
}
|
||||
|
||||
// Quantise to two decimal places. Equivalent to Python's `round(value, 2)`
|
||||
// but explicit so behaviour does not silently depend on the local C library.
|
||||
[[nodiscard]] auto quantise_to_centimetres(float value) -> float {
|
||||
return std::round(value * 100.0F) / 100.0F;
|
||||
}
|
||||
|
||||
[[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 {
|
||||
using Clock = std::chrono::system_clock;
|
||||
const auto now = Clock::now();
|
||||
const auto seconds = std::chrono::time_point_cast<std::chrono::seconds>(now);
|
||||
const auto millis = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
now - seconds
|
||||
).count();
|
||||
|
||||
const std::time_t epoch_seconds = Clock::to_time_t(seconds);
|
||||
std::tm broken_down{};
|
||||
#if defined(_WIN32)
|
||||
localtime_s(&broken_down, &epoch_seconds);
|
||||
#else
|
||||
localtime_r(&epoch_seconds, &broken_down);
|
||||
#endif
|
||||
|
||||
std::ostringstream stream;
|
||||
stream << std::put_time(&broken_down, "%H:%M:%S")
|
||||
<< '.' << std::setw(3) << std::setfill('0') << millis;
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
void append_u32_little_endian(std::vector<std::uint8_t>& buffer, std::uint32_t value) {
|
||||
buffer.push_back(static_cast<std::uint8_t>(value & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 8U) & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 16U) & 0xFFU));
|
||||
buffer.push_back(static_cast<std::uint8_t>((value >> 24U) & 0xFFU));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
auto observations_from_collection(
|
||||
const ipc::ResultCollection& collection,
|
||||
const FilterParams& filter
|
||||
) -> 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_centimetres(observation.crs),
|
||||
.dst = quantise_to_centimetres(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);
|
||||
std::vector<Observation> result;
|
||||
result.reserve(kept);
|
||||
for (std::size_t index = 0; index < kept; ++index) {
|
||||
result.push_back({
|
||||
.crs = quantise_to_centimetres(visible[index].crs),
|
||||
.dst = quantise_to_centimetres(visible[index].dst),
|
||||
});
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
auto build_payload_json(
|
||||
const std::vector<Observation>& observations,
|
||||
std::uint32_t protocol_version,
|
||||
std::uint32_t status
|
||||
) -> std::string {
|
||||
Json obs_array = Json::array();
|
||||
for (const auto& observation : observations) {
|
||||
obs_array.push_back({
|
||||
{"dst", observation.dst},
|
||||
{"crs", observation.crs},
|
||||
});
|
||||
}
|
||||
|
||||
const Json root{
|
||||
{"ver", protocol_version},
|
||||
{"tim", format_timestamp_now()},
|
||||
{"sts", status},
|
||||
{"obs", std::move(obs_array)},
|
||||
};
|
||||
return root.dump();
|
||||
}
|
||||
|
||||
auto encode_packet(
|
||||
const std::string& payload_json,
|
||||
std::uint32_t device_id
|
||||
) -> std::vector<std::uint8_t> {
|
||||
const auto payload_size = static_cast<std::uint32_t>(payload_json.size());
|
||||
std::vector<std::uint8_t> packet;
|
||||
packet.reserve(static_cast<std::size_t>(8U) + payload_json.size());
|
||||
|
||||
append_u32_little_endian(packet, device_id);
|
||||
append_u32_little_endian(packet, payload_size);
|
||||
const auto* bytes = reinterpret_cast<const std::uint8_t*>(payload_json.data());
|
||||
packet.insert(packet.end(), bytes, bytes + payload_json.size());
|
||||
return packet;
|
||||
}
|
||||
|
||||
} // namespace radar::locator
|
||||
@@ -0,0 +1,496 @@
|
||||
#include "locator/tcp_server.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include <mutex>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <system_error>
|
||||
#include <utility>
|
||||
|
||||
#include <arpa/inet.h>
|
||||
#include <fcntl.h>
|
||||
#include <netdb.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/tcp.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace radar::locator {
|
||||
namespace {
|
||||
|
||||
using Json = nlohmann::json;
|
||||
|
||||
constexpr std::size_t kPacketHeaderSize = 8U; // device_id u32 LE + payload_len u32 LE.
|
||||
|
||||
// Best-effort full-write helper: loops over write() until everything is sent
|
||||
// or an error occurs. Returns false on socket error or peer disconnect.
|
||||
[[nodiscard]] auto write_all(int socket_fd, const std::uint8_t* data, std::size_t size) -> bool {
|
||||
std::size_t written = 0;
|
||||
while (written < size) {
|
||||
const auto chunk = ::send(
|
||||
socket_fd,
|
||||
data + written,
|
||||
size - written,
|
||||
MSG_NOSIGNAL
|
||||
);
|
||||
if (chunk < 0) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (chunk == 0) {
|
||||
return false;
|
||||
}
|
||||
written += static_cast<std::size_t>(chunk);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Best-effort exact-read helper. Returns false if the peer closed the socket
|
||||
// or an unrecoverable error occurred before all bytes were read.
|
||||
[[nodiscard]] auto read_exact(int socket_fd, std::uint8_t* data, std::size_t size) -> bool {
|
||||
std::size_t consumed = 0;
|
||||
while (consumed < size) {
|
||||
const auto chunk = ::recv(socket_fd, data + consumed, size - consumed, 0);
|
||||
if (chunk < 0) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
if (chunk == 0) {
|
||||
return false;
|
||||
}
|
||||
consumed += static_cast<std::size_t>(chunk);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
[[nodiscard]] auto decode_u32_little_endian(const std::uint8_t* bytes) -> std::uint32_t {
|
||||
return static_cast<std::uint32_t>(bytes[0])
|
||||
| (static_cast<std::uint32_t>(bytes[1]) << 8U)
|
||||
| (static_cast<std::uint32_t>(bytes[2]) << 16U)
|
||||
| (static_cast<std::uint32_t>(bytes[3]) << 24U);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto format_peer(const sockaddr_storage& addr) -> std::string {
|
||||
std::array<char, INET6_ADDRSTRLEN> host_buffer{};
|
||||
std::array<char, NI_MAXSERV> port_buffer{};
|
||||
const auto err = ::getnameinfo(
|
||||
reinterpret_cast<const sockaddr*>(&addr),
|
||||
sizeof(addr),
|
||||
host_buffer.data(),
|
||||
host_buffer.size(),
|
||||
port_buffer.data(),
|
||||
port_buffer.size(),
|
||||
NI_NUMERICHOST | NI_NUMERICSERV
|
||||
);
|
||||
if (err != 0) {
|
||||
return "unknown";
|
||||
}
|
||||
return std::string(host_buffer.data()) + ':' + port_buffer.data();
|
||||
}
|
||||
|
||||
void apply_socket_keepalive(int socket_fd) {
|
||||
int yes = 1;
|
||||
(void)::setsockopt(socket_fd, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes));
|
||||
(void)::setsockopt(socket_fd, SOL_SOCKET, SO_KEEPALIVE, &yes, sizeof(yes));
|
||||
}
|
||||
|
||||
void shutdown_and_close(int& socket_fd) {
|
||||
if (socket_fd < 0) {
|
||||
return;
|
||||
}
|
||||
(void)::shutdown(socket_fd, SHUT_RDWR);
|
||||
(void)::close(socket_fd);
|
||||
socket_fd = -1;
|
||||
}
|
||||
|
||||
void log_warning(const std::string& message) {
|
||||
std::cerr << "locator: " << message << '\n';
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// ----- ClientQueue ----------------------------------------------------------
|
||||
|
||||
ClientQueue::ClientQueue(std::size_t capacity) : capacity_(std::max<std::size_t>(1U, capacity)) {}
|
||||
|
||||
auto ClientQueue::try_push(std::vector<std::uint8_t> packet) -> bool {
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
if (closed_ || queue_.size() >= capacity_) {
|
||||
return false;
|
||||
}
|
||||
queue_.push_back(std::move(packet));
|
||||
}
|
||||
not_empty_.notify_one();
|
||||
return true;
|
||||
}
|
||||
|
||||
auto ClientQueue::wait_pop() -> std::optional<std::vector<std::uint8_t>> {
|
||||
std::unique_lock<std::mutex> guard(mutex_);
|
||||
not_empty_.wait(guard, [this]() { return closed_ || !queue_.empty(); });
|
||||
if (queue_.empty()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
auto packet = std::move(queue_.front());
|
||||
queue_.pop_front();
|
||||
return packet;
|
||||
}
|
||||
|
||||
void ClientQueue::close() {
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
if (closed_) {
|
||||
return;
|
||||
}
|
||||
closed_ = true;
|
||||
}
|
||||
not_empty_.notify_all();
|
||||
}
|
||||
|
||||
auto ClientQueue::is_closed() const -> bool {
|
||||
std::lock_guard<std::mutex> guard(mutex_);
|
||||
return closed_;
|
||||
}
|
||||
|
||||
// ----- ClientSession --------------------------------------------------------
|
||||
|
||||
ClientSession::ClientSession(
|
||||
int socket_fd,
|
||||
std::string peer_name,
|
||||
std::size_t queue_capacity,
|
||||
std::uint32_t max_payload_bytes
|
||||
)
|
||||
: socket_fd_(socket_fd),
|
||||
peer_name_(std::move(peer_name)),
|
||||
queue_(queue_capacity),
|
||||
max_payload_bytes_(max_payload_bytes) {}
|
||||
|
||||
ClientSession::~ClientSession() {
|
||||
request_stop();
|
||||
join();
|
||||
shutdown_and_close(socket_fd_);
|
||||
}
|
||||
|
||||
void ClientSession::start(std::atomic<double>& shared_vlc_slot) {
|
||||
writer_thread_ = std::thread([this]() { writer_loop(); });
|
||||
reader_thread_ = std::thread([this, &shared_vlc_slot]() { reader_loop(shared_vlc_slot); });
|
||||
}
|
||||
|
||||
void ClientSession::enqueue(std::vector<std::uint8_t> packet) {
|
||||
if (stop_requested_.load(std::memory_order_acquire)) {
|
||||
return;
|
||||
}
|
||||
if (!queue_.try_push(std::move(packet))) {
|
||||
log_warning("disconnecting client " + peer_name_ + " after outbound queue overflow");
|
||||
request_stop();
|
||||
}
|
||||
}
|
||||
|
||||
void ClientSession::request_stop() {
|
||||
if (stop_requested_.exchange(true, std::memory_order_acq_rel)) {
|
||||
return;
|
||||
}
|
||||
queue_.close();
|
||||
// Wake any blocking recv() in the reader thread.
|
||||
if (socket_fd_ >= 0) {
|
||||
(void)::shutdown(socket_fd_, SHUT_RDWR);
|
||||
}
|
||||
}
|
||||
|
||||
void ClientSession::join() {
|
||||
if (writer_thread_.joinable()) {
|
||||
writer_thread_.join();
|
||||
}
|
||||
if (reader_thread_.joinable()) {
|
||||
reader_thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
auto ClientSession::has_exited() const -> bool {
|
||||
return exited_.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
auto ClientSession::peer_name() const -> const std::string& {
|
||||
return peer_name_;
|
||||
}
|
||||
|
||||
void ClientSession::writer_loop() {
|
||||
while (!stop_requested_.load(std::memory_order_acquire)) {
|
||||
auto packet = queue_.wait_pop();
|
||||
if (!packet.has_value()) {
|
||||
break;
|
||||
}
|
||||
if (!write_all(socket_fd_, packet->data(), packet->size())) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
request_stop();
|
||||
// Exited flag is set once both threads finish; reader_loop sets it.
|
||||
}
|
||||
|
||||
void ClientSession::reader_loop(std::atomic<double>& shared_vlc_slot) {
|
||||
std::array<std::uint8_t, kPacketHeaderSize> header_buffer{};
|
||||
std::vector<std::uint8_t> payload_buffer;
|
||||
|
||||
while (!stop_requested_.load(std::memory_order_acquire)) {
|
||||
if (!read_exact(socket_fd_, header_buffer.data(), header_buffer.size())) {
|
||||
break;
|
||||
}
|
||||
const auto payload_len = decode_u32_little_endian(header_buffer.data() + 4U);
|
||||
if (payload_len > max_payload_bytes_) {
|
||||
log_warning(
|
||||
"closing client " + peer_name_ + " after payload size "
|
||||
+ std::to_string(payload_len) + " exceeded the configured limit"
|
||||
);
|
||||
break;
|
||||
}
|
||||
|
||||
payload_buffer.assign(payload_len, std::uint8_t{0});
|
||||
if (payload_len > 0U && !read_exact(socket_fd_, payload_buffer.data(), payload_len)) {
|
||||
break;
|
||||
}
|
||||
|
||||
try {
|
||||
const auto json = Json::parse(payload_buffer.begin(), payload_buffer.end());
|
||||
if (json.is_object()) {
|
||||
const auto found = json.find("vlc");
|
||||
if (found != json.end() && found->is_number()) {
|
||||
const double value = found->get<double>();
|
||||
if (std::isfinite(value)) {
|
||||
shared_vlc_slot.store(value, std::memory_order_release);
|
||||
}
|
||||
}
|
||||
}
|
||||
} catch (const Json::parse_error& error) {
|
||||
log_warning(
|
||||
"ignoring malformed packet from " + peer_name_ + ": " + error.what()
|
||||
);
|
||||
}
|
||||
}
|
||||
request_stop();
|
||||
exited_.store(true, std::memory_order_release);
|
||||
}
|
||||
|
||||
// ----- TcpServer ------------------------------------------------------------
|
||||
|
||||
TcpServer::TcpServer(LocatorServerConfig config) : config_(std::move(config)) {
|
||||
latest_socket_speed_.store(
|
||||
std::numeric_limits<double>::quiet_NaN(),
|
||||
std::memory_order_relaxed
|
||||
);
|
||||
}
|
||||
|
||||
TcpServer::~TcpServer() {
|
||||
stop();
|
||||
}
|
||||
|
||||
void TcpServer::start() {
|
||||
if (running_.exchange(true, std::memory_order_acq_rel)) {
|
||||
return;
|
||||
}
|
||||
|
||||
addrinfo hints{};
|
||||
hints.ai_family = AF_UNSPEC;
|
||||
hints.ai_socktype = SOCK_STREAM;
|
||||
hints.ai_flags = AI_PASSIVE;
|
||||
|
||||
addrinfo* resolved = nullptr;
|
||||
const auto port_str = std::to_string(config_.port);
|
||||
const auto gai = ::getaddrinfo(
|
||||
config_.host.c_str(),
|
||||
port_str.c_str(),
|
||||
&hints,
|
||||
&resolved
|
||||
);
|
||||
if (gai != 0 || resolved == nullptr) {
|
||||
running_.store(false, std::memory_order_release);
|
||||
throw std::runtime_error(
|
||||
"locator: getaddrinfo failed for " + config_.host + ":" + port_str
|
||||
+ " (" + ::gai_strerror(gai) + ")"
|
||||
);
|
||||
}
|
||||
|
||||
int fd = -1;
|
||||
for (addrinfo* candidate = resolved; candidate != nullptr; candidate = candidate->ai_next) {
|
||||
fd = ::socket(candidate->ai_family, candidate->ai_socktype, candidate->ai_protocol);
|
||||
if (fd < 0) {
|
||||
continue;
|
||||
}
|
||||
int yes = 1;
|
||||
(void)::setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));
|
||||
if (::bind(fd, candidate->ai_addr, candidate->ai_addrlen) == 0) {
|
||||
break;
|
||||
}
|
||||
::close(fd);
|
||||
fd = -1;
|
||||
}
|
||||
::freeaddrinfo(resolved);
|
||||
|
||||
if (fd < 0) {
|
||||
running_.store(false, std::memory_order_release);
|
||||
throw std::runtime_error(
|
||||
"locator: failed to bind " + config_.host + ":" + port_str
|
||||
+ " (" + std::string(std::strerror(errno)) + ")"
|
||||
);
|
||||
}
|
||||
|
||||
if (::listen(fd, 16) < 0) {
|
||||
::close(fd);
|
||||
running_.store(false, std::memory_order_release);
|
||||
throw std::runtime_error(
|
||||
"locator: listen() failed (" + std::string(std::strerror(errno)) + ")"
|
||||
);
|
||||
}
|
||||
|
||||
listen_fd_ = fd;
|
||||
acceptor_thread_ = std::thread([this]() { acceptor_loop(); });
|
||||
}
|
||||
|
||||
void TcpServer::stop() {
|
||||
if (!running_.exchange(false, std::memory_order_acq_rel)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (listen_fd_ >= 0) {
|
||||
(void)::shutdown(listen_fd_, SHUT_RDWR);
|
||||
(void)::close(listen_fd_);
|
||||
listen_fd_ = -1;
|
||||
}
|
||||
|
||||
if (acceptor_thread_.joinable()) {
|
||||
acceptor_thread_.join();
|
||||
}
|
||||
|
||||
std::vector<std::unique_ptr<ClientSession>> sessions;
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(clients_mutex_);
|
||||
sessions = std::move(clients_);
|
||||
clients_.clear();
|
||||
}
|
||||
for (auto& session : sessions) {
|
||||
session->request_stop();
|
||||
}
|
||||
for (auto& session : sessions) {
|
||||
session->join();
|
||||
}
|
||||
}
|
||||
|
||||
auto TcpServer::is_running() const -> bool {
|
||||
return running_.load(std::memory_order_acquire);
|
||||
}
|
||||
|
||||
void TcpServer::publish(const ipc::ResultCollection& collection, const FilterParams& filter) {
|
||||
if (!running_.load(std::memory_order_acquire)) {
|
||||
return;
|
||||
}
|
||||
|
||||
const auto observations = observations_from_collection(collection, filter);
|
||||
const auto payload_json = build_payload_json(observations, config_.protocol_version);
|
||||
auto packet = encode_packet(payload_json, config_.device_id);
|
||||
cache_latest_packet(packet);
|
||||
broadcast_packet(packet);
|
||||
}
|
||||
|
||||
auto TcpServer::latest_socket_speed() const -> std::optional<double> {
|
||||
const double value = latest_socket_speed_.load(std::memory_order_acquire);
|
||||
if (std::isnan(value)) {
|
||||
return std::nullopt;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
void TcpServer::acceptor_loop() {
|
||||
while (running_.load(std::memory_order_acquire)) {
|
||||
sockaddr_storage peer_addr{};
|
||||
socklen_t peer_len = sizeof(peer_addr);
|
||||
const int client_fd = ::accept(
|
||||
listen_fd_,
|
||||
reinterpret_cast<sockaddr*>(&peer_addr),
|
||||
&peer_len
|
||||
);
|
||||
if (client_fd < 0) {
|
||||
if (errno == EINTR) {
|
||||
continue;
|
||||
}
|
||||
// Listening socket closed during shutdown produces EBADF/EINVAL; bail.
|
||||
break;
|
||||
}
|
||||
|
||||
reap_finished_clients();
|
||||
|
||||
apply_socket_keepalive(client_fd);
|
||||
auto session = std::make_unique<ClientSession>(
|
||||
client_fd,
|
||||
format_peer(peer_addr),
|
||||
config_.client_queue_size,
|
||||
config_.max_payload_bytes
|
||||
);
|
||||
const auto snapshot = latest_packet_copy();
|
||||
if (snapshot.has_value()) {
|
||||
session->enqueue(*snapshot);
|
||||
}
|
||||
session->start(latest_socket_speed_);
|
||||
enroll_client(std::move(session));
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::enroll_client(std::unique_ptr<ClientSession> session) {
|
||||
std::lock_guard<std::mutex> guard(clients_mutex_);
|
||||
clients_.push_back(std::move(session));
|
||||
}
|
||||
|
||||
void TcpServer::broadcast_packet(const std::vector<std::uint8_t>& packet) {
|
||||
std::lock_guard<std::mutex> guard(clients_mutex_);
|
||||
for (auto& client : clients_) {
|
||||
client->enqueue(packet);
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::reap_finished_clients() {
|
||||
std::vector<std::unique_ptr<ClientSession>> to_join;
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(clients_mutex_);
|
||||
auto first_dead = std::partition(
|
||||
clients_.begin(),
|
||||
clients_.end(),
|
||||
[](const std::unique_ptr<ClientSession>& session) {
|
||||
return !session->has_exited();
|
||||
}
|
||||
);
|
||||
for (auto it = first_dead; it != clients_.end(); ++it) {
|
||||
to_join.push_back(std::move(*it));
|
||||
}
|
||||
clients_.erase(first_dead, clients_.end());
|
||||
}
|
||||
for (auto& session : to_join) {
|
||||
session->request_stop();
|
||||
session->join();
|
||||
}
|
||||
}
|
||||
|
||||
void TcpServer::cache_latest_packet(std::vector<std::uint8_t> packet) {
|
||||
std::lock_guard<std::mutex> guard(latest_packet_mutex_);
|
||||
latest_packet_ = std::move(packet);
|
||||
}
|
||||
|
||||
auto TcpServer::latest_packet_copy() const -> std::optional<std::vector<std::uint8_t>> {
|
||||
std::lock_guard<std::mutex> guard(latest_packet_mutex_);
|
||||
return latest_packet_;
|
||||
}
|
||||
|
||||
} // namespace radar::locator
|
||||
+123
-17
@@ -2,11 +2,15 @@
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <exception>
|
||||
#include <random>
|
||||
#include <string>
|
||||
#include <string_view>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
@@ -19,6 +23,60 @@ namespace {
|
||||
constexpr std::uint32_t kNativeAcquireMaxAttempts = 3U;
|
||||
constexpr auto kNativeSweepResponseTimeout = std::chrono::milliseconds(1500);
|
||||
|
||||
// One synthetic GPR reflector. `range_m` is its physical depth, `reflection`
|
||||
// is the dimensionless complex reflection coefficient (|Γ| ≤ 1).
|
||||
struct MockTarget {
|
||||
float range_m;
|
||||
float reflection_magnitude;
|
||||
};
|
||||
|
||||
// Three reflectors at GPR-typical depths: a strong near-surface scatterer,
|
||||
// a mid-depth target, and a weak deeper one. The magnitudes are tuned so the
|
||||
// summed S21 stays within unit modulus across the sweep band.
|
||||
constexpr std::array<MockTarget, 3> kMockTargets = {{
|
||||
{0.45F, 0.55F},
|
||||
{1.30F, 0.30F},
|
||||
{2.90F, 0.18F},
|
||||
}};
|
||||
|
||||
// Group velocity in moderately wet soil (≈ c / sqrt(εr), εr ≈ 4). The choice
|
||||
// is what maps reflector depth to round-trip phase delay; it is held constant
|
||||
// to keep the simulator deterministic.
|
||||
constexpr float kGroundVelocityMps = 1.5e8F;
|
||||
|
||||
// Soil attenuation grows with frequency. Calibrated so a target at 3 m sees
|
||||
// roughly −20 dB extra loss at 6 GHz on top of geometric spreading.
|
||||
constexpr float kAttenuationCoeffPerMeterAtRefHz = 0.22F;
|
||||
constexpr float kAttenuationReferenceHz = 6e9F;
|
||||
|
||||
// Antenna mismatch dominates S11: simulate one shallow reflection right at
|
||||
// the connector, plus a small amount of cross-coupling from S21 targets.
|
||||
constexpr float kS11ConnectorReflection = 0.55F;
|
||||
constexpr float kS11ConnectorRangeM = 0.02F;
|
||||
constexpr float kS11CrossCouplingFactor = 0.06F;
|
||||
|
||||
// Noise floor in linear voltage units. Real LibreVNA hits ~ −90 dB at 1 kHz
|
||||
// IFBW; pick something a touch noisier so the GPR processor has to work.
|
||||
constexpr float kNoiseAmplitudeLinear = 0.004F;
|
||||
|
||||
// Minimum simulated dwell so 0-point or pathological configs don't busy-loop.
|
||||
constexpr auto kMockMinimumSweepDuration = std::chrono::microseconds(50);
|
||||
|
||||
// Compute the dwell time the mock pretends to spend on the device. Mirrors
|
||||
// the real LibreVNA contract: per-point dwell ≈ 1 / IFBW. Capped so absurd
|
||||
// configs (IFBW ≈ 0 or huge sweeps) cannot freeze the producer for hours.
|
||||
[[nodiscard]] auto mock_target_sweep_duration(const config::RadarSweepSettings& sweep)
|
||||
-> std::chrono::nanoseconds {
|
||||
const float points = std::max(1.0F, static_cast<float>(sweep.points));
|
||||
const float if_bw = std::max(1.0F, sweep.if_bandwidth_hz);
|
||||
const double seconds = static_cast<double>(points) / static_cast<double>(if_bw);
|
||||
const auto duration_ns = std::chrono::nanoseconds(
|
||||
static_cast<std::chrono::nanoseconds::rep>(seconds * 1e9)
|
||||
);
|
||||
constexpr auto kHardCap = std::chrono::seconds(5);
|
||||
return std::clamp<std::chrono::nanoseconds>(duration_ns, kMockMinimumSweepDuration, kHardCap);
|
||||
}
|
||||
|
||||
[[nodiscard]] auto is_retryable_native_acquire_error(std::string_view message) -> bool {
|
||||
constexpr std::array<std::string_view, 5> kRetryableSubstrings = {
|
||||
"Timeout waiting for expected LibreVNA packet type",
|
||||
@@ -109,34 +167,82 @@ auto LibreVnaMinimalDriver::acquire_sweep() -> SweepTrace {
|
||||
}
|
||||
|
||||
auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
|
||||
// Synthesise a frequency-domain VNA response containing several discrete
|
||||
// reflectors at known depths, plus light Gaussian noise. The sweep dwells
|
||||
// for a realistic duration derived from the configured IF bandwidth so
|
||||
// downstream stages cannot be flooded faster than a real device would
|
||||
// produce data.
|
||||
const auto started_at = std::chrono::steady_clock::now();
|
||||
const auto target_duration = mock_target_sweep_duration(settings_.sweep);
|
||||
|
||||
SweepTrace trace{};
|
||||
trace.frequency_hz.reserve(settings_.sweep.points);
|
||||
trace.s11.reserve(settings_.sweep.points);
|
||||
trace.s21.reserve(settings_.sweep.points);
|
||||
|
||||
const auto span_hz = settings_.sweep.stop_hz - settings_.sweep.start_hz;
|
||||
const auto denominator = settings_.sweep.points > 1U ? static_cast<float>(settings_.sweep.points - 1U) : 1.0F;
|
||||
const float span_hz = settings_.sweep.stop_hz - settings_.sweep.start_hz;
|
||||
const float denominator =
|
||||
settings_.sweep.points > 1U ? static_cast<float>(settings_.sweep.points - 1U) : 1.0F;
|
||||
|
||||
// A tiny per-sweep range drift gives the rendered B-scan a visible motion
|
||||
// signature so the simulator does not look frozen.
|
||||
const float range_drift_m =
|
||||
0.01F * std::sin(0.07F * static_cast<float>(sweep_index_));
|
||||
|
||||
// Deterministic-per-sweep noise so two consecutive frames look distinct
|
||||
// but the test stays reproducible for any given sweep index.
|
||||
std::mt19937 noise_engine(
|
||||
static_cast<std::uint32_t>(0x9E3779B9ULL ^ sweep_index_)
|
||||
);
|
||||
std::normal_distribution<float> noise_dist(0.0F, kNoiseAmplitudeLinear);
|
||||
|
||||
for (std::uint32_t point = 0; point < settings_.sweep.points; ++point) {
|
||||
const auto ratio = static_cast<float>(point) / denominator;
|
||||
const auto frequency_hz = settings_.sweep.start_hz + span_hz * ratio;
|
||||
const auto phase = 2.0F * detail::kPi * (frequency_hz / std::max(settings_.mock_signal_hz, 1.0F)) +
|
||||
static_cast<float>(sweep_index_) * 0.05F;
|
||||
const auto envelope = 0.6F + 0.4F * std::sin(0.5F * phase);
|
||||
const float ratio = static_cast<float>(point) / denominator;
|
||||
const float frequency_hz = settings_.sweep.start_hz + span_hz * ratio;
|
||||
const float frequency_scale = frequency_hz / kAttenuationReferenceHz;
|
||||
|
||||
ipc::Complex32 sample{};
|
||||
sample.re = envelope * std::cos(phase);
|
||||
sample.im = envelope * std::sin(phase);
|
||||
std::complex<float> s21_total{0.0F, 0.0F};
|
||||
std::complex<float> s11_total{0.0F, 0.0F};
|
||||
|
||||
const auto reflection_phase = 0.7F * phase + 0.35F;
|
||||
const auto reflection_envelope = 0.15F + 0.1F * std::cos(0.25F * phase);
|
||||
ipc::Complex32 reflection{};
|
||||
reflection.re = reflection_envelope * std::cos(reflection_phase);
|
||||
reflection.im = reflection_envelope * std::sin(reflection_phase);
|
||||
for (const auto& target : kMockTargets) {
|
||||
const float range_m = target.range_m + range_drift_m;
|
||||
// Round-trip phase: 2π·f·(2R/v).
|
||||
const float round_trip_phase =
|
||||
2.0F * detail::kPi * frequency_hz * (2.0F * range_m / kGroundVelocityMps);
|
||||
// Geometric spreading: 1/(1 + R) keeps near-zero ranges finite.
|
||||
const float spreading = 1.0F / (1.0F + range_m);
|
||||
// Frequency-dependent soil attenuation in linear amplitude.
|
||||
const float attenuation =
|
||||
std::exp(-kAttenuationCoeffPerMeterAtRefHz * range_m * frequency_scale);
|
||||
|
||||
const std::complex<float> contribution = std::polar<float>(
|
||||
target.reflection_magnitude * spreading * attenuation,
|
||||
-round_trip_phase
|
||||
);
|
||||
s21_total += contribution;
|
||||
s11_total += kS11CrossCouplingFactor * contribution;
|
||||
}
|
||||
|
||||
// Antenna mismatch dominates the near-field S11 response.
|
||||
const float antenna_phase =
|
||||
2.0F * detail::kPi * frequency_hz * (2.0F * kS11ConnectorRangeM / kGroundVelocityMps);
|
||||
s11_total += std::polar<float>(kS11ConnectorReflection, -antenna_phase);
|
||||
|
||||
// Independent noise per channel; complex variance ≈ kNoiseAmplitudeLinear².
|
||||
s21_total += std::complex<float>(noise_dist(noise_engine), noise_dist(noise_engine));
|
||||
s11_total += std::complex<float>(noise_dist(noise_engine), noise_dist(noise_engine));
|
||||
|
||||
trace.frequency_hz.push_back(frequency_hz);
|
||||
trace.s11.push_back(reflection);
|
||||
trace.s21.push_back(sample);
|
||||
trace.s11.push_back({.re = s11_total.real(), .im = s11_total.imag()});
|
||||
trace.s21.push_back({.re = s21_total.real(), .im = s21_total.imag()});
|
||||
}
|
||||
|
||||
// Honour the IFBW-derived dwell time. If generation already took longer
|
||||
// than the simulated device would have needed (huge `points` × CPU jitter)
|
||||
// we skip the sleep so the producer does not fall further behind.
|
||||
const auto elapsed = std::chrono::steady_clock::now() - started_at;
|
||||
if (elapsed < target_duration) {
|
||||
std::this_thread::sleep_for(target_duration - elapsed);
|
||||
}
|
||||
|
||||
return trace;
|
||||
|
||||
Reference in New Issue
Block a user