added timing

data_acq_and_processing/processing/data_processor/include/data_processor.hpp

@@ -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;

data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp

@@ -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;

data_acq_and_processing/processing/data_processor/src/data_processor.cpp

@@ -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{};
     {

data_acq_and_processing/processing/data_processor/src/main.cpp

@@ -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;

data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp

@@ -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");

data_acq_and_processing/processing/locator/include/locator/locator_config.hpp

@@ -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

data_acq_and_processing/processing/locator/include/locator/observation.hpp

@@ -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

data_acq_and_processing/processing/locator/include/locator/payload_builder.hpp

@@ -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

data_acq_and_processing/processing/locator/include/locator/tcp_server.hpp

@@ -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

data_acq_and_processing/processing/locator/src/payload_builder.cpp

@@ -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

data_acq_and_processing/processing/locator/src/tcp_server.cpp

@@ -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