added timing
This commit is contained in:
Ayzen
@@ -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
|
||||
Reference in New Issue
Block a user