diff --git a/SDK_VERSION b/SDK_VERSION index a803cc22..a5510516 100644 --- a/SDK_VERSION +++ b/SDK_VERSION @@ -1 +1 @@ -0.14.0 +0.15.0 diff --git a/data_stream_foxglove_bridge/foxglove_dialog.hpp b/data_stream_foxglove_bridge/foxglove_dialog.hpp index e167fd57..7d177fd7 100644 --- a/data_stream_foxglove_bridge/foxglove_dialog.hpp +++ b/data_stream_foxglove_bridge/foxglove_dialog.hpp @@ -106,8 +106,11 @@ class FoxgloveDialog : public PJ::DialogPluginTyped { wd.setSelectedItems("topicsList", selected_topic_names_); } - // OK button: enabled only when connected and channels are selected - wd.setOkEnabled(connected_ && !selected_topic_names_.empty()); + // OK button: enabled when connected. Selecting channels is optional — + // selected ones are always subscribed (the eager floor); on a + // lazy-subscription host the remaining channels subscribe on demand as + // curves are dropped. + wd.setOkEnabled(connected_.load()); return wd.toJson(); } @@ -188,6 +191,13 @@ class FoxgloveDialog : public PJ::DialogPluginTyped { void onAccepted(std::string_view /*json*/) override { // Do NOT disconnect — the source's onStart() will steal the socket. snapshotSelectedChannels(); + // Snapshot the FULL catalog too: the stolen socket never re-receives the + // advertise burst, and the lazy-subscription source advertises all + // channels to the host from this snapshot (config key "all_channels"). + { + std::lock_guard lock(channels_mutex_); + all_channels_snapshot_ = channels_; + } } void onRejected() override { disconnect(); @@ -200,20 +210,29 @@ class FoxgloveDialog : public PJ::DialogPluginTyped { pj::array_policy::arrayLimitToJson(cfg, static_cast(max_array_size_), clamp_large_arrays_); cfg["use_timestamp"] = use_timestamp_; - // Use the snapshot — channels_ may be cleared by disconnect() - nlohmann::json channels_json = nlohmann::json::array(); - for (const auto& ch : selected_channels_snapshot_) { - channels_json.push_back({ + // Use the snapshots — channels_ may be cleared by disconnect() + const auto to_json = [](const DiscoveredChannel& ch) { + return nlohmann::json{ {"id", ch.id}, {"topic", ch.topic}, {"encoding", ch.encoding}, {"schema_name", ch.schema_name}, {"schema", ch.schema}, {"schema_encoding", ch.schema_encoding}, - }); + }; + }; + nlohmann::json channels_json = nlohmann::json::array(); + for (const auto& ch : selected_channels_snapshot_) { + channels_json.push_back(to_json(ch)); } cfg["channels"] = channels_json; + nlohmann::json all_channels_json = nlohmann::json::array(); + for (const auto& ch : all_channels_snapshot_) { + all_channels_json.push_back(to_json(ch)); + } + cfg["all_channels"] = all_channels_json; + return cfg.dump(); } @@ -229,21 +248,32 @@ class FoxgloveDialog : public PJ::DialogPluginTyped { clamp_large_arrays_ = array_limit.clamp(); use_timestamp_ = cfg.value("use_timestamp", false); - // Restore previously selected topic names and snapshot + // Restore previously selected topic names and snapshots + const auto from_json = [](const nlohmann::json& ch_json) { + DiscoveredChannel ch; + ch.id = ch_json.value("id", uint64_t{0}); + ch.topic = ch_json.value("topic", std::string{}); + ch.encoding = ch_json.value("encoding", std::string{}); + ch.schema_name = ch_json.value("schema_name", std::string{}); + ch.schema = ch_json.value("schema", std::string{}); + ch.schema_encoding = ch_json.value("schema_encoding", std::string{}); + return ch; + }; if (cfg.contains("channels") && cfg["channels"].is_array()) { selected_topic_names_.clear(); selected_channels_snapshot_.clear(); for (const auto& ch_json : cfg["channels"]) { if (ch_json.contains("topic") && ch_json["topic"].is_string()) { selected_topic_names_.push_back(ch_json["topic"].get()); - DiscoveredChannel ch; - ch.id = ch_json.value("id", uint64_t{0}); - ch.topic = ch_json.value("topic", std::string{}); - ch.encoding = ch_json.value("encoding", std::string{}); - ch.schema_name = ch_json.value("schema_name", std::string{}); - ch.schema = ch_json.value("schema", std::string{}); - ch.schema_encoding = ch_json.value("schema_encoding", std::string{}); - selected_channels_snapshot_.push_back(std::move(ch)); + selected_channels_snapshot_.push_back(from_json(ch_json)); + } + } + } + if (cfg.contains("all_channels") && cfg["all_channels"].is_array()) { + all_channels_snapshot_.clear(); + for (const auto& ch_json : cfg["all_channels"]) { + if (ch_json.contains("topic") && ch_json["topic"].is_string()) { + all_channels_snapshot_.push_back(from_json(ch_json)); } } } @@ -355,6 +385,7 @@ class FoxgloveDialog : public PJ::DialogPluginTyped { std::vector channels_; std::vector selected_topic_names_; std::vector selected_channels_snapshot_; + std::vector all_channels_snapshot_; bool channels_dirty_ = true; std::atomic tick_dirty_ = false; }; diff --git a/data_stream_foxglove_bridge/foxglove_protocol.hpp b/data_stream_foxglove_bridge/foxglove_protocol.hpp index 3cd17884..27d388e7 100644 --- a/data_stream_foxglove_bridge/foxglove_protocol.hpp +++ b/data_stream_foxglove_bridge/foxglove_protocol.hpp @@ -2,7 +2,10 @@ #include #include +#include +#include #include +#include #include namespace PJ::FoxgloveProtocol { @@ -115,4 +118,33 @@ inline bool isUsableChannel(const ChannelInfo& ch) { return classifyChannel(ch).supported; } +/// Result of one declarative subscription reconcile (lazy subscription). +struct SubscriptionOps { + std::vector> to_subscribe; ///< (fresh sub_id, channel_id) + std::vector to_unsubscribe; ///< sub ids to drop +}; + +/// Pure diff between the live subscription map (sub_id -> channel_id) and the +/// DESIRED channel-id set: channels newly desired get fresh sub ids from +/// `next_sub_id`; subscriptions whose channel is no longer desired are +/// dropped. Idempotent: an empty diff yields empty ops. +inline SubscriptionOps computeSubscriptionOps( + const std::map& current, const std::set& desired_channels, uint32_t& next_sub_id) { + SubscriptionOps ops; + std::set covered; + for (const auto& [sub_id, channel_id] : current) { + if (desired_channels.count(channel_id) == 0) { + ops.to_unsubscribe.push_back(sub_id); + } else { + covered.insert(channel_id); + } + } + for (const uint64_t channel_id : desired_channels) { + if (covered.count(channel_id) == 0) { + ops.to_subscribe.emplace_back(next_sub_id++, channel_id); + } + } + return ops; +} + } // namespace PJ::FoxgloveProtocol diff --git a/data_stream_foxglove_bridge/foxglove_source.cpp b/data_stream_foxglove_bridge/foxglove_source.cpp index 31ba5c5d..c2096476 100644 --- a/data_stream_foxglove_bridge/foxglove_source.cpp +++ b/data_stream_foxglove_bridge/foxglove_source.cpp @@ -12,7 +12,9 @@ #include #include #include +#include #include +#include #include #include @@ -41,7 +43,7 @@ class FoxgloveSource : public PJ::StreamSourceBase { } uint64_t extraCapabilities() const override { - return PJ::kCapabilityDelegatedIngest | PJ::kCapabilityHasDialog; + return PJ::kCapabilityDelegatedIngest | PJ::kCapabilityHasDialog | PJ::kCapabilityLazySubscription; } std::string saveConfig() const override { @@ -70,19 +72,32 @@ class FoxgloveSource : public PJ::StreamSourceBase { selected_channels_.clear(); if (cfg.contains("channels") && cfg["channels"].is_array()) { for (const auto& ch : cfg["channels"]) { - ChannelInfo info; - info.id = ch.value("id", uint64_t{0}); - info.topic = ch.value("topic", ""); - info.encoding = ch.value("encoding", ""); - info.schema_name = ch.value("schema_name", ""); - info.schema = ch.value("schema", ""); - info.schema_encoding = ch.value("schema_encoding", ""); + ChannelInfo info = channelFromConfigJson(ch); if (!info.topic.empty()) { selected_channels_.push_back(std::move(info)); } } } + // Full channel catalog captured by the dialog from the initial advertise + // burst (the stolen socket never re-receives it). Basis for the lazy-mode + // advertise-all; falls back to the selected channels when a config saved + // by an older dialog lacks it. + channel_catalog_.clear(); + if (cfg.contains("all_channels") && cfg["all_channels"].is_array()) { + for (const auto& ch : cfg["all_channels"]) { + ChannelInfo info = channelFromConfigJson(ch); + if (!info.topic.empty()) { + channel_catalog_[info.id] = std::move(info); + } + } + } + if (channel_catalog_.empty()) { + for (const auto& ch : selected_channels_) { + channel_catalog_[ch.id] = ch; + } + } + // Steal the live socket from the dialog (it stays connected on accept). socket_ = dialog_.takeSocket(); @@ -131,13 +146,43 @@ class FoxgloveSource : public PJ::StreamSourceBase { // When the socket is stolen from the dialog it is already connected and the server // already sent its initial "advertise" burst — it will not re-send it. - // Subscribe directly using the channel info captured by the dialog. - subscribeToSelectedChannels(); + // + // Lazy mode: advertise the FULL catalog to the host without subscribing; + // the host drives per-channel subscriptions from consumer demand, with + // the dialog-selected channels as an always-on eager floor. On a pre-0.15 + // host the advertise fails with a distinct error and we fall back to the + // eager subscribe-selected behavior unchanged. + lazy_mode_ = advertiseCatalogToHost(); + if (!lazy_mode_ && selected_channels_.empty()) { + // Old host without lazy-mode support and no dialog-selected channels — + // there is nothing to subscribe to and nothing will ever appear later, + // so fail loudly here instead of streaming nothing forever. Mirrors + // the ROS 2 source's `!lazy_mode_ && selected_topics_.empty()` guard. + socket_->stop(); + socket_.reset(); + return PJ::unexpected("no Foxglove channels selected"); + } + if (lazy_mode_) { + reconcileSubscriptions(); + } else { + subscribeToSelectedChannels(); + } connected_ = true; return PJ::okStatus(); } + PJ::Status onActiveTopicsChanged(PJ::Span active_topics) override { + host_active_.clear(); + for (const std::string_view name : active_topics) { + host_active_.insert(std::string(name)); + } + if (lazy_mode_ && connected_) { + reconcileSubscriptions(); + } + return PJ::okStatus(); + } + PJ::Status onPoll() override { // Drain text messages (advertise/unadvertise) queued from the WebSocket callback thread. // ensureParserBinding() must be called from the poll thread, not the callback thread. @@ -159,10 +204,16 @@ class FoxgloveSource : public PJ::StreamSourceBase { connected_ = true; reconnect_pending_ = false; reconnect_tick_ = 0; - // Reset subscription state so new advertise messages create fresh bindings + // Reset subscription state so new advertise messages create fresh bindings. + // In lazy mode the catalog also resets — the fresh advertise burst + // rebuilds it and re-advertises to the host; host_active_ persists, + // so wanted topics re-subscribe automatically on the reconcile. subscriptions_.clear(); binding_by_subscription_.clear(); advertised_channels_.clear(); + if (lazy_mode_) { + channel_catalog_.clear(); + } next_subscription_id_ = 1; runtimeHost().reportMessage(PJ::DataSourceMessageLevel::kInfo, "Reconnected to Foxglove bridge"); } else if (socket_->getReadyState() == ix::ReadyState::Closed) { @@ -224,41 +275,63 @@ class FoxgloveSource : public PJ::StreamSourceBase { selected_channels_.clear(); subscriptions_.clear(); binding_by_subscription_.clear(); + channel_catalog_.clear(); + host_active_.clear(); + lazy_mode_ = false; next_subscription_id_ = 1; } private: - // Create a parser binding for a supported channel and record it for its - // subscription id. Returns an error string on binding failure, std::nullopt - // on success. Shared by both subscribe paths (stolen-socket and fresh-connect). - std::optional bindChannel(const ChannelInfo& ch, uint32_t sub_id) { - const ChannelRoute route = classifyChannel(ch); + static ChannelInfo channelFromConfigJson(const nlohmann::json& ch) { + ChannelInfo info; + info.id = ch.value("id", uint64_t{0}); + info.topic = ch.value("topic", ""); + info.encoding = ch.value("encoding", ""); + info.schema_name = ch.value("schema_name", ""); + info.schema = ch.value("schema", ""); + info.schema_encoding = ch.value("schema_encoding", ""); + return info; + } + // Parser config forwarded with every binding/advertise for one route — + // identical for bindChannel and advertiseCatalogToHost so the host's + // per-topic binding dedupe sees one consistent request per topic. + std::string parserConfigFor(const ChannelRoute& route) const { nlohmann::json parser_cfg; pj::array_policy::arrayLimitToJson(parser_cfg, array_limit_); parser_cfg["use_timestamp"] = use_timestamp_; parser_cfg["use_embedded_timestamp"] = use_timestamp_; parser_cfg["schema_encoding"] = route.parser_encoding; + return parser_cfg.dump(); + } - // Foxglove base64-encodes binary schemas (the protobuf FileDescriptorSet) in - // the advertise JSON, while text schemas (ros2msg/omgidl) arrive verbatim. - // `decoded` must outlive the ensureParserBinding call below — the host - // consumes the schema span synchronously, same as data_load_mcap. - std::string decoded; - PJ::Span schema_span; + // Foxglove base64-encodes binary schemas (the protobuf FileDescriptorSet) in + // the advertise JSON, while text schemas (ros2msg/omgidl) arrive verbatim. + // `decoded_storage` must outlive the host call consuming the span. + static PJ::Span schemaSpanFor( + const ChannelInfo& ch, const ChannelRoute& route, std::string& decoded_storage) { if (route.schema_is_base64) { - decoded = PJ::base64::decode(ch.schema); - schema_span = PJ::Span(reinterpret_cast(decoded.data()), decoded.size()); - } else { - schema_span = PJ::Span(reinterpret_cast(ch.schema.data()), ch.schema.size()); + decoded_storage = PJ::base64::decode(ch.schema); + return PJ::Span(reinterpret_cast(decoded_storage.data()), decoded_storage.size()); } + return PJ::Span(reinterpret_cast(ch.schema.data()), ch.schema.size()); + } + + // Create a parser binding for a supported channel and record it for its + // subscription id. Returns an error string on binding failure, std::nullopt + // on success. Shared by both subscribe paths (stolen-socket and fresh-connect). + std::optional bindChannel(const ChannelInfo& ch, uint32_t sub_id) { + const ChannelRoute route = classifyChannel(ch); + + std::string decoded; + const PJ::Span schema_span = schemaSpanFor(ch, route, decoded); auto binding = runtimeHost().ensureParserBinding({ .topic_name = ch.topic, .parser_encoding = route.parser_encoding, .type_name = ch.schema_name, .schema = schema_span, - .parser_config_json = parser_cfg.dump(), + .parser_config_json = parserConfigFor(route), }); if (!binding) { return ch.topic + " (" + route.parser_encoding + "): " + binding.error(); @@ -267,6 +340,96 @@ class FoxgloveSource : public PJ::StreamSourceBase { return std::nullopt; } + // Advertise every supported catalog channel to the host WITHOUT + // subscribing. Returns false when the host predates lazy subscription + // (distinct setAdvertisedTopics error) — callers then fall back to eager. + bool advertiseCatalogToHost() { + std::vector requests; + std::vector decoded_storage; + std::vector config_storage; + requests.reserve(channel_catalog_.size()); + decoded_storage.reserve(channel_catalog_.size()); + config_storage.reserve(channel_catalog_.size()); + + for (const auto& [channel_id, ch] : channel_catalog_) { + const ChannelRoute route = classifyChannel(ch); + if (!route.supported) { + continue; + } + decoded_storage.emplace_back(); + config_storage.push_back(parserConfigFor(route)); + requests.push_back( + PJ::ParserBindingRequest{ + .topic_name = ch.topic, + .parser_encoding = route.parser_encoding, + .type_name = ch.schema_name, + .schema = schemaSpanFor(ch, route, decoded_storage.back()), + .parser_config_json = config_storage.back(), + }); + } + + auto status = + runtimeHost().setAdvertisedTopics(PJ::Span(requests.data(), requests.size())); + if (!status) { + runtimeHost().reportMessage( + PJ::DataSourceMessageLevel::kInfo, "Lazy subscription unavailable (" + status.error() + + ") — falling back to eager subscription of selected channels"); + return false; + } + return true; + } + + // True when a channel's topic was explicitly selected in the dialog — the + // eager floor, always subscribed regardless of host demand. + bool isEagerTopic(const std::string& topic) const { + for (const auto& sel : selected_channels_) { + if (sel.topic == topic) { + return true; + } + } + return false; + } + + // Declarative reconcile: desired = dialog-selected eager floor ∪ the + // host's active set, mapped to supported catalog channels. Newly desired + // channels get bound + subscribed; no-longer-desired ones unsubscribed + // (their host-side bindings and data persist). + void reconcileSubscriptions() { + std::set desired_channels; + for (const auto& [channel_id, ch] : channel_catalog_) { + if (!classifyChannel(ch).supported) { + continue; + } + if (host_active_.count(ch.topic) > 0 || isEagerTopic(ch.topic)) { + desired_channels.insert(channel_id); + } + } + + const SubscriptionOps ops = computeSubscriptionOps(subscriptions_, desired_channels, next_subscription_id_); + if (!ops.to_unsubscribe.empty()) { + socket_->sendText(buildUnsubscribeMessage(ops.to_unsubscribe)); + for (const uint32_t sub_id : ops.to_unsubscribe) { + subscriptions_.erase(sub_id); + binding_by_subscription_.erase(sub_id); + } + } + if (!ops.to_subscribe.empty()) { + std::vector parser_errors; + for (const auto& [sub_id, channel_id] : ops.to_subscribe) { + const auto ch = channel_catalog_.find(channel_id); + if (ch == channel_catalog_.end()) { + continue; + } + subscriptions_[sub_id] = channel_id; + if (auto err = bindChannel(ch->second, sub_id)) { + parser_errors.push_back(*err); + } + socket_->sendText(buildSubscribeMessage({{sub_id, channel_id}})); + } + reportParserErrors(parser_errors); + } + } + void reportParserErrors(const std::vector& parser_errors) { if (parser_errors.empty()) { return; @@ -328,16 +491,15 @@ class FoxgloveSource : public PJ::StreamSourceBase { // Track server-advertised channels for unadvertise advertised_channels_[ch.id] = ch.topic; + channel_catalog_[ch.id] = ch; - // Only subscribe to channels that were selected in the dialog - bool user_selected = false; - for (const auto& sel : selected_channels_) { - if (sel.topic == ch.topic) { - user_selected = true; - break; - } + // Lazy mode: subscriptions are driven by the reconcile below, not here. + if (lazy_mode_) { + continue; } - if (!user_selected) { + + // Only subscribe to channels that were selected in the dialog + if (!isEagerTopic(ch.topic)) { continue; } if (!classifyChannel(ch).supported) { @@ -356,6 +518,14 @@ class FoxgloveSource : public PJ::StreamSourceBase { } reportParserErrors(parser_errors); + + // Mid-stream advertise in lazy mode: refresh the host's catalog and + // reconcile — a newly advertised topic the host already wants (e.g. a + // pending layout curve) subscribes immediately. + if (lazy_mode_ && !channels_arr.empty()) { + (void)advertiseCatalogToHost(); + reconcileSubscriptions(); + } } // Handle unadvertise: server removed channels @@ -369,6 +539,7 @@ class FoxgloveSource : public PJ::StreamSourceBase { removed_topics.push_back(it->second); advertised_channels_.erase(it); } + channel_catalog_.erase(removed_id); // Clean up subscriptions referencing removed channels for (auto sub_it = subscriptions_.begin(); sub_it != subscriptions_.end();) { if (sub_it->second == removed_id) { @@ -385,6 +556,11 @@ class FoxgloveSource : public PJ::StreamSourceBase { msg += " " + t; } runtimeHost().reportMessage(PJ::DataSourceMessageLevel::kWarning, msg); + // Lazy mode: report the shrunken catalog so the host marks the + // removed topics unavailable (their data persists host-side). + if (lazy_mode_) { + (void)advertiseCatalogToHost(); + } } } @@ -441,6 +617,13 @@ class FoxgloveSource : public PJ::StreamSourceBase { std::map advertised_channels_; // channel_id -> topic name uint32_t next_subscription_id_ = 1; + // Lazy subscription: full channel catalog (advertised to the host), the + // host's desired-active topic set, and whether the host accepted lazy mode. + // All touched only on the poll/start thread (the extension contract). + std::map channel_catalog_; + std::set host_active_; + bool lazy_mode_ = false; + std::mutex queue_mutex_; std::queue message_queue_; std::mutex text_queue_mutex_; diff --git a/data_stream_foxglove_bridge/test_scripts/foxglove_image_server.py b/data_stream_foxglove_bridge/test_scripts/foxglove_image_server.py index 7408ffaf..904d4c30 100644 --- a/data_stream_foxglove_bridge/test_scripts/foxglove_image_server.py +++ b/data_stream_foxglove_bridge/test_scripts/foxglove_image_server.py @@ -189,8 +189,11 @@ async def on_message(self, websocket, message): channel_id = sub.get("channelId") if sub_id is not None and channel_id in CHANNEL_BY_ID: self.clients[websocket][channel_id] = sub_id - print(f" subscribe → channel ids {list(self.clients[websocket].keys())}") + import time as _time + print(f"\n SUBSCRIBE → channel ids {list(self.clients[websocket].keys())} at wallclock {_time.strftime('%H:%M:%S')}") elif op == "unsubscribe": + import time as _time + print(f"\n UNSUBSCRIBE → subscription ids {msg.get('subscriptionIds', [])} at wallclock {_time.strftime('%H:%M:%S')}") for sub_id in msg.get("subscriptionIds", []): self.clients[websocket] = { ch_id: s_id diff --git a/data_stream_foxglove_bridge/test_scripts/foxglove_stress_server.py b/data_stream_foxglove_bridge/test_scripts/foxglove_stress_server.py new file mode 100644 index 00000000..1e413a05 --- /dev/null +++ b/data_stream_foxglove_bridge/test_scripts/foxglove_stress_server.py @@ -0,0 +1,714 @@ +#!/usr/bin/env python3 +""" +Foxglove WebSocket stress server — instrumented fake server for E2E-testing +PJ4's lazy-subscription feature (data_stream_foxglove_bridge). + +Publishes a wide, deliberately heterogeneous mix of channels so a test can +subscribe/unsubscribe individual topics and observe that: + - unsubscribed channels cost ~0 CPU/bandwidth (nothing is even encoded) + - subscribing to a huge channel (e.g. /stress/cloud_huge, ~25 MB @ 2 Hz) + doesn't stall delivery of small/high-rate channels (e.g. the 10 Hz scalars) + +Channels advertised (all CDR / ros2msg, modelled on foxglove_image_server.py): + /stress/scalar_00 .. scalar_{N-1} std_msgs/msg/Float64 10 Hz, sine w/ per-topic phase + /stress/image sensor_msgs/msg/CompressedImage 10 Hz, ~50 KB JPEG + /stress/cloud_small sensor_msgs/msg/PointCloud2 2 Hz, ~64 KB + /stress/cloud_large sensor_msgs/msg/PointCloud2 2 Hz, ~5 MB + /stress/cloud_huge sensor_msgs/msg/PointCloud2 2 Hz, ~25 MB + /stress/tf tf2_msgs/msg/TFMessage 5 Hz, 2 transforms + +Only channels with at least one active subscription are ever encoded — this +mirrors the real "lazy subscription" contract under test: a client that never +subscribes to /stress/cloud_huge must never pay for it. + +Usage: + pip install foxglove-websocket pillow numpy + python3 foxglove_stress_server.py [--host HOST] [--port PORT] [--scalars N] +""" + +import argparse +import asyncio +import io +import json +import math +import struct +import time + +import numpy as np +import websockets +from PIL import Image, ImageDraw + +HOST = "localhost" +PORT = 8765 + +MESSAGE_DATA_OPCODE = 0x01 + +STATS_INTERVAL_S = 5.0 + +# High-resolution machine-readable event log for the E2E assertion checker. +# One JSON object per line: {"t": , "event": ..., "topic": ..., +# "sub_id": ...}. The stdout SUBSCRIBE/UNSUBSCRIBE prints are second-granular — +# too coarse for the <1s timing invariants — so this is the source of truth. +_EVENT_LOG_FH = None + + +def log_event(event, topic=None, sub_id=None): + if _EVENT_LOG_FH is None: + return + rec = {"t": time.time(), "event": event} + if topic is not None: + rec["topic"] = topic + if sub_id is not None: + rec["sub_id"] = sub_id + _EVENT_LOG_FH.write(json.dumps(rec) + "\n") + _EVENT_LOG_FH.flush() + + +# --------------------------------------------------------------------------- +# CDR encoding — a small generic writer. +# +# CDR alignment is relative to the *origin* of the encapsulated data, which +# begins right after the 4-byte encapsulation header (verified against +# rosx_introspection's NanoCDR decoder: `origin_ = buffer.data() + 4`). So the +# first byte written after the header is position 0, and every scalar aligns +# to its own size (1/4/8) relative to that position — NOT relative to the +# absolute start of the wire buffer (which is 4 bytes further along). +# --------------------------------------------------------------------------- + + +class CdrWriter: + """Minimal little-endian CDR (v1) writer with correct alignment tracking.""" + + def __init__(self): + # Encapsulation header: [representation_id:2][options:2], little-endian. + self._buf = bytearray(b"\x00\x01\x00\x00") + + def _pos(self) -> int: + """Offset relative to the CDR origin (right after the 4-byte header).""" + return len(self._buf) - 4 + + def _align(self, size: int) -> None: + pad = (-self._pos()) % size + if pad: + self._buf += b"\x00" * pad + + def u8(self, value: int) -> None: + self._buf += struct.pack(" None: + self.u8(1 if value else 0) + + def u32(self, value: int) -> None: + self._align(4) + self._buf += struct.pack(" None: + self._align(4) + self._buf += struct.pack(" None: + self._align(4) + self._buf += struct.pack(" None: + self._align(8) + self._buf += struct.pack(" None: + """CDR string: uint32 length (incl. NUL) + bytes + NUL terminator.""" + raw = s.encode("utf-8") + b"\x00" + self.u32(len(raw)) + self._buf += raw + + def u8_sequence(self, data: bytes) -> None: + """sequence: uint32 count + raw bytes (uint8 needs no alignment).""" + self.u32(len(data)) + self._buf += data + + def f32_sequence_raw(self, arr: np.ndarray) -> None: + """Append a pre-packed little-endian float32 array as raw payload bytes, + after aligning to 4 (the caller is responsible for having emitted the + matching uint32 element count beforehand, e.g. via u32()).""" + self._align(4) + self._buf += arr.astype(" bytes: + return bytes(self._buf) + + +def stamp_from_t(t: float) -> tuple: + ns = int(t * 1e9) + return ns // 1_000_000_000, ns % 1_000_000_000 + + +def write_header(w: CdrWriter, t: float, frame_id: str) -> None: + """std_msgs/msg/Header: builtin_interfaces/Time stamp + string frame_id.""" + sec, nanosec = stamp_from_t(t) + w.i32(sec) + w.u32(nanosec) + w.string(frame_id) + + +# --------------------------------------------------------------------------- +# ROS2 message schemas (.msg format) — flattened with all nested types, in +# the same "80x'=' separator + MSG: " convention as foxglove_image_server.py. +# Note: schemaName carries the /msg/ segment; the schema *body* type +# references (field types and MSG: headers) do not, matching upstream rosmsg +# text and the existing image_server precedent. +# --------------------------------------------------------------------------- + +SEP = "=" * 80 + +HEADER_DEPS = f"""\ +{SEP} +MSG: std_msgs/Header +builtin_interfaces/Time stamp +string frame_id +{SEP} +MSG: builtin_interfaces/Time +int32 sec +uint32 nanosec +""" + +FLOAT64_SCHEMA = "float64 data\n" + +COMPRESSED_IMAGE_SCHEMA = f"""\ +std_msgs/Header header +string format +uint8[] data +{HEADER_DEPS}""" + +POINTCLOUD2_SCHEMA = f"""\ +std_msgs/Header header +uint32 height +uint32 width +sensor_msgs/PointField[] fields +bool is_bigendian +uint32 point_step +uint32 row_step +uint8[] data +bool is_dense +{HEADER_DEPS}\ +{SEP} +MSG: sensor_msgs/PointField +uint8 INT8 = 1 +uint8 UINT8 = 2 +uint8 INT16 = 3 +uint8 UINT16 = 4 +uint8 INT32 = 5 +uint8 UINT32 = 6 +uint8 FLOAT32 = 7 +uint8 FLOAT64 = 8 + +string name +uint32 offset +uint8 datatype +uint32 count +""" + +TFMESSAGE_SCHEMA = f"""\ +geometry_msgs/TransformStamped[] transforms +{SEP} +MSG: geometry_msgs/TransformStamped +std_msgs/Header header +string child_frame_id +geometry_msgs/Transform transform +{HEADER_DEPS}\ +{SEP} +MSG: geometry_msgs/Transform +geometry_msgs/Vector3 translation +geometry_msgs/Quaternion rotation +{SEP} +MSG: geometry_msgs/Vector3 +float64 x +float64 y +float64 z +{SEP} +MSG: geometry_msgs/Quaternion +float64 x +float64 y +float64 z +float64 w +""" + +POINTFIELD_FLOAT32 = 7 # sensor_msgs/msg/PointField.FLOAT32 + + +# --------------------------------------------------------------------------- +# Per-message encoders +# --------------------------------------------------------------------------- + + +def encode_float64_cdr(value: float) -> bytes: + w = CdrWriter() + w.f64(value) + return w.bytes() + + +WIDTH, HEIGHT = 320, 240 + + +def make_frame(t: float) -> bytes: + """~50 KB JPEG: animated gradient + per-frame noise (keeps JPEG entropy up + so the compressed size stays in a narrow, predictable band) + bouncing ball.""" + xs = np.arange(WIDTH) + ys = np.arange(HEIGHT) + r_shift = int(127 + 127 * math.sin(t * 0.5)) + g_shift = int(127 + 127 * math.sin(t * 0.7 + 2.0)) + b_shift = int(127 + 127 * math.sin(t * 0.3 + 4.0)) + + img = np.zeros((HEIGHT, WIDTH, 3), dtype=np.int16) + img[:, :, 0] = (r_shift + xs[None, :]) % 256 + img[:, :, 1] = (g_shift + ys[:, None]) % 256 + img[:, :, 2] = b_shift + + rng = np.random.default_rng(int(t * 1000) & 0xFFFFFFFF) + noise = rng.integers(-40, 40, size=(HEIGHT, WIDTH, 3)) + img = np.clip(img + noise, 0, 255).astype(np.uint8) + + pil_img = Image.fromarray(img, "RGB") + draw = ImageDraw.Draw(pil_img) + bx = int(WIDTH * 0.5 + (WIDTH * 0.4) * math.sin(t * 1.2)) + by = int(HEIGHT * 0.5 + (HEIGHT * 0.4) * math.cos(t * 0.9)) + draw.ellipse([bx - 15, by - 15, bx + 15, by + 15], fill=(255, 255, 255)) + draw.rectangle([0, 0, 160, 20], fill=(0, 0, 0)) + draw.text((4, 4), f"t={t:.2f}s", fill=(255, 255, 0)) + + buf = io.BytesIO() + pil_img.save(buf, format="JPEG", quality=93) + return buf.getvalue() + + +def encode_compressed_image_cdr(t: float) -> bytes: + """CDR-encode sensor_msgs/msg/CompressedImage.""" + jpeg_bytes = make_frame(t) + w = CdrWriter() + write_header(w, t, "camera") + w.string("jpeg") + w.u8_sequence(jpeg_bytes) + return w.bytes() + + +class PointCloudSource: + """Precomputes a base point cloud once (cheap RNG draw), then cheaply + animates it per-frame with a rotation — avoids re-drawing millions of + random numbers on every tick for the huge cloud.""" + + def __init__(self, n_points: int, radius: float, seed: int): + rng = np.random.default_rng(seed) + vecs = rng.normal(size=(n_points, 3)).astype(np.float64) + vecs /= np.linalg.norm(vecs, axis=1, keepdims=True) + shell = np.cbrt(rng.uniform(0.15, 1.0, size=(n_points, 1))) + self.base = (vecs * shell * radius) + self.n_points = n_points + + def points_at(self, t: float) -> np.ndarray: + c, s = math.cos(t * 0.3), math.sin(t * 0.3) + rot = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) + return self.base @ rot.T + + +def encode_pointcloud2_cdr(t: float, frame_id: str, points: np.ndarray) -> bytes: + """CDR-encode sensor_msgs/msg/PointCloud2 with packed float32 x,y,z.""" + n_points = points.shape[0] + point_step = 12 # 3 x float32 + row_step = point_step * n_points + data = points.astype(" bytes: + """CDR-encode tf2_msgs/msg/TFMessage: sequence.""" + w = CdrWriter() + w.u32(len(transforms)) + for frame_id, child_frame_id, translation, rotation in transforms: + write_header(w, t, frame_id) + w.string(child_frame_id) + for v in translation: + w.f64(v) + for v in rotation: + w.f64(v) + return w.bytes() + + +def build_binary_frame(subscription_id: int, log_time_ns: int, cdr: bytes) -> bytes: + """[opcode:1][subscription_id:4 LE][log_time_ns:8 LE][cdr_payload]""" + return struct.pack(" {"messages": int, "bytes": int} + self.stats = {ch["id"]: {"messages": 0, "bytes": 0} for ch in self.channels} + self._last_stats = {cid: dict(v) for cid, v in self.stats.items()} + self._last_stats_time = time.monotonic() + + # -- connection lifecycle ------------------------------------------------ + + async def handler(self, websocket): + client_id = id(websocket) + self.clients[websocket] = {} + print(f"[+] Client connected: {client_id}") + + log_event("connect") + await websocket.send(json.dumps({"op": "advertise", "channels": self.channels})) + log_event("advertise") + print(f" advertise → {len(self.channels)} channels " + f"({len(self.scalar_ids)} scalars, image, " + f"{len(self.cloud_ids)} clouds, tf)") + + task = asyncio.create_task(self._readvertise_until_subscribed(websocket)) + self._readvertise_tasks[websocket] = task + + try: + async for message in websocket: + await self.on_message(websocket, message) + except websockets.exceptions.ConnectionClosed: + pass + finally: + task.cancel() + self._readvertise_tasks.pop(websocket, None) + del self.clients[websocket] + print(f"[-] Client disconnected: {client_id}") + + async def _readvertise_until_subscribed(self, websocket): + try: + while True: + await asyncio.sleep(2.0) + if not self.clients.get(websocket): + await websocket.send(json.dumps({"op": "advertise", "channels": self.channels})) + print(" re-advertise → waiting for subscribe") + else: + break + except (asyncio.CancelledError, websockets.exceptions.ConnectionClosed): + pass + + async def on_message(self, websocket, message): + if isinstance(message, bytes): + return + try: + msg = json.loads(message) + except json.JSONDecodeError: + return + + op = msg.get("op", "") + if op == "subscribe": + for sub in msg.get("subscriptions", []): + sub_id = sub.get("id") + channel_id = sub.get("channelId") + if sub_id is not None and channel_id in self.channel_by_id: + self.clients[websocket][channel_id] = sub_id + log_event("subscribe", self.channel_by_id[channel_id]["topic"], sub_id) + topics = [self.channel_by_id[c]["topic"] for c in self.clients[websocket]] + print(f"\n SUBSCRIBE → {topics} at wallclock {time.strftime('%H:%M:%S')}") + elif op == "unsubscribe": + sub_ids = msg.get("subscriptionIds", []) + unsub_topics = [ + self.channel_by_id[ch_id]["topic"] + for ch_id, s_id in self.clients[websocket].items() + if s_id in sub_ids + ] + for ch_id, s_id in self.clients[websocket].items(): + if s_id in sub_ids: + log_event("unsubscribe", self.channel_by_id[ch_id]["topic"], s_id) + print(f"\n UNSUBSCRIBE → {unsub_topics} at wallclock {time.strftime('%H:%M:%S')}") + self.clients[websocket] = { + ch_id: s_id + for ch_id, s_id in self.clients[websocket].items() + if s_id not in sub_ids + } + + # -- subscription helpers ------------------------------------------------- + + def _subscribers(self, channel_id: int): + """Yield (websocket, subscription_id) for every client subscribed to channel_id.""" + for websocket, subs in list(self.clients.items()): + sub_id = subs.get(channel_id) + if sub_id is not None: + yield websocket, sub_id + + def _has_subscribers(self, channel_id: int) -> bool: + return any(channel_id in subs for subs in self.clients.values()) + + async def _publish(self, channel_id: int, cdr: bytes) -> int: + """Send `cdr` to every current subscriber of channel_id. Returns + subscriber count (0 means the caller shouldn't have encoded at all — + callers are expected to check _has_subscribers() first).""" + log_time_ns = time.time_ns() + count = 0 + for websocket, sub_id in self._subscribers(channel_id): + frame = build_binary_frame(sub_id, log_time_ns, cdr) + try: + await websocket.send(frame) + count += 1 + except websockets.exceptions.ConnectionClosed: + pass + if count: + self.stats[channel_id]["messages"] += 1 + self.stats[channel_id]["bytes"] += len(cdr) * count + return count + + # -- emit loops ------------------------------------------------------------ + + async def emit_scalars(self): + """10 Hz sine wave per scalar topic, per-topic phase offset.""" + interval = 0.1 + t = 0.0 + n = len(self.scalar_ids) + while True: + await asyncio.sleep(interval) + t += interval + for idx, channel_id in enumerate(self.scalar_ids): + if not self._has_subscribers(channel_id): + continue + phase = (2 * math.pi * idx) / max(n, 1) + value = math.sin(2 * math.pi * 0.5 * t + phase) + cdr = encode_float64_cdr(value) + await self._publish(channel_id, cdr) + + async def emit_image(self): + """10 Hz ~50 KB JPEG frame.""" + interval = 0.1 + t = 0.0 + while True: + await asyncio.sleep(interval) + t += interval + if not self._has_subscribers(self.image_id): + continue + cdr = encode_compressed_image_cdr(t) + await self._publish(self.image_id, cdr) + + async def emit_clouds(self): + """2 Hz PointCloud2 for small/large/huge — only encodes clouds that + currently have at least one subscriber (this is the crux of the + lazy-subscription contract: cloud_huge must be free when idle).""" + interval = 0.5 + t = 0.0 + while True: + await asyncio.sleep(interval) + t += interval + for name, channel_id in self.cloud_ids.items(): + if not self._has_subscribers(channel_id): + continue + source = self.cloud_sources[channel_id] + points = source.points_at(t) + cdr = encode_pointcloud2_cdr(t, "map", points) + await self._publish(channel_id, cdr) + + async def emit_tf(self): + """5 Hz TFMessage with 2 animated transforms.""" + interval = 0.2 + t = 0.0 + while True: + await asyncio.sleep(interval) + t += interval + if not self._has_subscribers(self.tf_id): + continue + transforms = [ + ( + "world", "robot1", + (2.0 * math.cos(t * 0.5), 2.0 * math.sin(t * 0.5), 0.0), + (0.0, 0.0, math.sin(t * 0.25), math.cos(t * 0.25)), + ), + ( + "world", "robot2", + (-1.5 * math.sin(t * 0.3), 1.5 * math.cos(t * 0.3), 0.5), + (0.0, 0.0, math.sin(-t * 0.15), math.cos(-t * 0.15)), + ), + ] + cdr = encode_tf_message_cdr(t, transforms) + await self._publish(self.tf_id, cdr) + + async def report_stats(self): + """Periodic instrumentation: per-channel subscriber count + throughput + since the last report, so an E2E test (or a human) can eyeball that + unsubscribed channels are truly at zero cost.""" + while True: + await asyncio.sleep(STATS_INTERVAL_S) + now = time.monotonic() + elapsed = now - self._last_stats_time + self._last_stats_time = now + + lines = [] + for ch in self.channels: + cid = ch["id"] + cur = self.stats[cid] + prev = self._last_stats[cid] + d_msgs = cur["messages"] - prev["messages"] + d_bytes = cur["bytes"] - prev["bytes"] + self._last_stats[cid] = dict(cur) + n_subs = sum(1 for subs in self.clients.values() if cid in subs) + if n_subs == 0 and d_msgs == 0: + continue + rate = d_msgs / elapsed if elapsed > 0 else 0.0 + bw = d_bytes / elapsed if elapsed > 0 else 0.0 + lines.append( + f" {ch['topic']:<24s} subs={n_subs} " + f"{rate:6.2f} msg/s {bw / 1024:9.1f} KB/s " + f"(total {cur['messages']} msgs, {cur['bytes'] / 1e6:.2f} MB)" + ) + + if lines: + print(f"\n [stats @ {time.strftime('%H:%M:%S')}] active channels:") + print("\n".join(lines)) + else: + print(f"\n [stats @ {time.strftime('%H:%M:%S')}] idle (no subscriptions)") + + +async def main(host: str, port: int, num_scalars: int): + server = FoxgloveStressServer(num_scalars) + print(f"Foxglove stress server listening on ws://{host}:{port}") + print(f"Channels: {len(server.channels)} total") + print(f" /stress/scalar_00..{num_scalars - 1:02d} std_msgs/msg/Float64 10 Hz") + print(f" /stress/image sensor_msgs/msg/CompressedImage 10 Hz ~50 KB") + print(f" /stress/cloud_small sensor_msgs/msg/PointCloud2 2 Hz ~64 KB") + print(f" /stress/cloud_large sensor_msgs/msg/PointCloud2 2 Hz ~5 MB") + print(f" /stress/cloud_huge sensor_msgs/msg/PointCloud2 2 Hz ~25 MB") + print(f" /stress/tf tf2_msgs/msg/TFMessage 5 Hz") + print("Nothing is encoded/sent for a channel until a client subscribes to it.") + print("Ctrl+C to stop\n") + + async with websockets.serve( + server.handler, + host, + port, + subprotocols=["foxglove.sdk.v1"], + max_size=None, # cloud_huge frames (~25 MB) exceed the websockets default limit + ): + await asyncio.gather( + server.emit_scalars(), + server.emit_image(), + server.emit_clouds(), + server.emit_tf(), + server.report_stats(), + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Foxglove stress server — lazy-subscription E2E fixture") + parser.add_argument("--host", default=HOST) + parser.add_argument("--port", type=int, default=PORT) + parser.add_argument("--scalars", type=int, default=20, help="number of scalar topics (default: 20)") + parser.add_argument("--event-log", default=None, + help="write high-resolution JSONL subscribe/unsubscribe events here (E2E checker source of truth)") + args = parser.parse_args() + + if args.event_log: + _EVENT_LOG_FH = open(args.event_log, "w") + print(f"Event log → {args.event_log}") + + try: + asyncio.run(main(args.host, args.port, args.scalars)) + except KeyboardInterrupt: + print("\nStopped.") + finally: + if _EVENT_LOG_FH is not None: + _EVENT_LOG_FH.close() diff --git a/data_stream_foxglove_bridge/tests/foxglove_protocol_test.cpp b/data_stream_foxglove_bridge/tests/foxglove_protocol_test.cpp index 51f3d569..64728c31 100644 --- a/data_stream_foxglove_bridge/tests/foxglove_protocol_test.cpp +++ b/data_stream_foxglove_bridge/tests/foxglove_protocol_test.cpp @@ -245,4 +245,41 @@ TEST(Base64Test, BinarySafe) { EXPECT_EQ(static_cast(decoded[3]), 0x00); } +// --- computeSubscriptionOps (lazy-subscription reconcile diff) --- + +TEST(SubscriptionOpsTest, EmptyToDesiredSubscribesAll) { + uint32_t next_id = 1; + const auto ops = computeSubscriptionOps({}, {10, 20}, next_id); + ASSERT_EQ(ops.to_subscribe.size(), 2u); + EXPECT_TRUE(ops.to_unsubscribe.empty()); + EXPECT_EQ(ops.to_subscribe[0].first, 1u); + EXPECT_EQ(ops.to_subscribe[1].first, 2u); + EXPECT_EQ(next_id, 3u); +} + +TEST(SubscriptionOpsTest, NoChangeYieldsEmptyOps) { + uint32_t next_id = 5; + const auto ops = computeSubscriptionOps({{1, 10}, {2, 20}}, {10, 20}, next_id); + EXPECT_TRUE(ops.to_subscribe.empty()); + EXPECT_TRUE(ops.to_unsubscribe.empty()); + EXPECT_EQ(next_id, 5u); +} + +TEST(SubscriptionOpsTest, MixedAddAndRemove) { + uint32_t next_id = 3; + // Subscribed to 10 and 20; host now wants 20 and 30. + const auto ops = computeSubscriptionOps({{1, 10}, {2, 20}}, {20, 30}, next_id); + ASSERT_EQ(ops.to_unsubscribe.size(), 1u); + EXPECT_EQ(ops.to_unsubscribe[0], 1u); + ASSERT_EQ(ops.to_subscribe.size(), 1u); + EXPECT_EQ(ops.to_subscribe[0], (std::pair{3u, 30u})); +} + +TEST(SubscriptionOpsTest, DesiredEmptyUnsubscribesAll) { + uint32_t next_id = 7; + const auto ops = computeSubscriptionOps({{1, 10}, {2, 20}}, {}, next_id); + EXPECT_TRUE(ops.to_subscribe.empty()); + ASSERT_EQ(ops.to_unsubscribe.size(), 2u); +} + } // namespace diff --git a/data_stream_ros2/distro/src/data_stream_ros2.cpp b/data_stream_ros2/distro/src/data_stream_ros2.cpp index 47ac71b7..157cb9fc 100644 --- a/data_stream_ros2/distro/src/data_stream_ros2.cpp +++ b/data_stream_ros2/distro/src/data_stream_ros2.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -26,9 +27,12 @@ #include #include #include +#include #include +#include #include #include +#include #include #include @@ -55,7 +59,7 @@ class Ros2StreamSource : public PJ::StreamSourceBase { } uint64_t extraCapabilities() const override { - return PJ::kCapabilityDelegatedIngest | PJ::kCapabilityHasDialog; + return PJ::kCapabilityDelegatedIngest | PJ::kCapabilityHasDialog | PJ::kCapabilityLazySubscription; } std::string saveConfig() const override { @@ -96,10 +100,6 @@ class Ros2StreamSource : public PJ::StreamSourceBase { } } } - if (selected_topics_.empty()) { - return PJ::unexpected("no ROS 2 topics selected"); - } - // Stash the parser_config sub-object as a string for every ensureBinding // call. parser_ros::loadConfig accepts unknown keys silently, so unknown // future options pass through harmlessly. @@ -121,20 +121,20 @@ class Ros2StreamSource : public PJ::StreamSourceBase { executor_ = std::make_unique(exec_opts, 2); executor_->add_node(node_); - for (const auto& [topic, type] : selected_topics_) { - const auto qos = ros2_streamer::adaptQosWaitingForPublishers(*node_, topic); - if (node_->count_publishers(topic) == 0) { - runtimeHost().reportMessage( - PJ::DataSourceMessageLevel::kWarning, - "No publishers visible for " + topic + " within discovery timeout — using default QoS"); - } - - auto callback = [this, topic](std::shared_ptr msg) { - enqueueMessage(topic, std::move(msg)); - }; + // Lazy mode: advertise the whole ROS graph to the host without + // subscribing; the host drives per-topic subscriptions from consumer + // demand, with the dialog selection as an always-on eager floor. On a + // pre-0.15 host the advertise fails with a distinct error and we keep + // today's eager subscribe-selected behavior — where an empty selection + // remains an error. + lazy_mode_ = advertiseGraphToHost(/*force=*/true); + if (!lazy_mode_ && selected_topics_.empty()) { + teardown(); + return PJ::unexpected("no ROS 2 topics selected"); + } - auto subscription = node_->create_generic_subscription(topic, type, qos, callback); - subscriptions_.emplace(topic, std::move(subscription)); + for (const auto& [topic, type] : selected_topics_) { + subscribeTopic(topic, type); } running_ = true; @@ -151,7 +151,29 @@ class Ros2StreamSource : public PJ::StreamSourceBase { return PJ::okStatus(); } + PJ::Status onActiveTopicsChanged(PJ::Span active_topics) override { + host_active_.clear(); + for (const std::string_view name : active_topics) { + host_active_.insert(std::string(name)); + } + if (lazy_mode_ && node_ != nullptr) { + reconcileSubscriptions(); + } + return PJ::okStatus(); + } + PJ::Status onPoll() override { + // Lazy mode: refresh graph discovery ~1 Hz (the dialog's cadence). A + // changed graph re-advertises the full topic set to the host and + // reconciles — a topic the host already wants (e.g. a restored layout + // curve) subscribes the moment its publisher appears. + if (lazy_mode_ && node_ != nullptr && ++discovery_tick_ >= kDiscoveryPollTicks) { + discovery_tick_ = 0; + if (advertiseGraphToHost(/*force=*/false)) { + reconcileSubscriptions(); + } + } + std::queue batch; { std::lock_guard lock(queue_mutex_); @@ -208,33 +230,41 @@ class Ros2StreamSource : public PJ::StreamSourceBase { message_queue_.push(std::move(pending)); } - PJ::ParserBindingHandle* ensureBinding(const std::string& topic) { - auto it = binding_cache_.find(topic); - if (it != binding_cache_.end()) { - return &it->second; - } - - std::string type_name; + // Message type for a topic: the dialog selection wins (it may pin one of + // several advertised types), else the discovered graph. + std::string typeFor(const std::string& topic) const { for (const auto& [t, ty] : selected_topics_) { if (t == topic) { - type_name = ty; - break; + return ty; } } - if (type_name.empty()) { - return nullptr; + if (auto it = advertised_types_.find(topic); it != advertised_types_.end()) { + return it->second; } + return {}; + } - std::string schema; + // Cached buildRos2Schema per type (typesupport introspection is not cheap + // and the advertise path touches every graph type). nullptr when the type's + // schema cannot be built; reported once per type. Cache entries are stable + // references for the life of the source (unordered_map never invalidates + // element references), so advertise spans can point straight into it. + const std::string* schemaFor(const std::string& type_name) { + if (auto it = schema_cache_.find(type_name); it != schema_cache_.end()) { + return it->second.empty() ? nullptr : &it->second; + } try { - schema = ros2_streamer::buildRos2Schema(type_name); + auto [it, _] = schema_cache_.emplace(type_name, ros2_streamer::buildRos2Schema(type_name)); + return &it->second; } catch (const std::exception& e) { + schema_cache_.emplace(type_name, std::string{}); runtimeHost().reportMessage( - PJ::DataSourceMessageLevel::kWarning, - "Failed to build schema for " + topic + " (" + type_name + "): " + e.what()); + PJ::DataSourceMessageLevel::kWarning, "Failed to build schema for type " + type_name + ": " + e.what()); return nullptr; } + } + std::string buildParserConfigString() const { nlohmann::json parser_config = nlohmann::json::object(); const std::string& base_parser_config = !parser_config_override_.empty() ? parser_config_override_ : parser_config_json_; @@ -245,14 +275,153 @@ class Ros2StreamSource : public PJ::StreamSourceBase { } } parser_config["schema_encoding"] = "ros2msg"; - const std::string parser_config_str = parser_config.dump(); + return parser_config.dump(); + } + + // Query the graph and, when it changed (or `force`), advertise the FULL + // topic set to the host. Returns true iff an advertise was sent and the + // host accepted it — false both for "no change" and for "host predates + // lazy subscription" (onStart's `force` call is what distinguishes the + // latter and decides lazy_mode_). + bool advertiseGraphToHost(bool force) { + std::map next_types; + try { + for (const auto& [topic, types] : node_->get_topic_names_and_types()) { + if (!types.empty()) { + next_types.emplace(topic, types.front()); + } + } + } catch (const std::exception& e) { + runtimeHost().reportMessage( + PJ::DataSourceMessageLevel::kWarning, std::string("ROS 2 graph discovery failed: ") + e.what()); + return false; + } + if (!force && next_types == advertised_types_) { + return false; + } + advertised_types_ = std::move(next_types); + + std::vector requests; + std::vector config_storage; + requests.reserve(advertised_types_.size()); + config_storage.reserve(advertised_types_.size()); + for (const auto& [topic, type] : advertised_types_) { + const std::string* schema = schemaFor(type); + if (schema == nullptr) { + continue; + } + config_storage.push_back(buildParserConfigString()); + requests.push_back( + PJ::ParserBindingRequest{ + .topic_name = topic, + .parser_encoding = "ros2msg", + .type_name = type, + .schema = PJ::Span(reinterpret_cast(schema->data()), schema->size()), + .parser_config_json = config_storage.back(), + }); + } + + auto status = + runtimeHost().setAdvertisedTopics(PJ::Span(requests.data(), requests.size())); + if (!status) { + runtimeHost().reportMessage( + PJ::DataSourceMessageLevel::kInfo, "Lazy subscription unavailable (" + status.error() + + ") — falling back to eager subscription of selected topics"); + return false; + } + return true; + } + + // Create one GenericSubscription (poll/start thread). Failures are + // reported, not fatal — the topic simply produces nothing. + void subscribeTopic(const std::string& topic, const std::string& type) { + if (subscriptions_.count(topic) != 0) { + return; + } + try { + const auto qos = ros2_streamer::adaptQosWaitingForPublishers(*node_, topic); + if (node_->count_publishers(topic) == 0) { + runtimeHost().reportMessage( + PJ::DataSourceMessageLevel::kWarning, + "No publishers visible for " + topic + " within discovery timeout — using default QoS"); + } + auto callback = [this, topic](std::shared_ptr msg) { + enqueueMessage(topic, std::move(msg)); + }; + subscriptions_.emplace(topic, node_->create_generic_subscription(topic, type, qos, callback)); + } catch (const std::exception& e) { + runtimeHost().reportMessage( + PJ::DataSourceMessageLevel::kWarning, "Failed to subscribe " + topic + ": " + e.what()); + } + } + + // Declarative reconcile: desired = dialog-selected eager floor ∪ the + // host's active set, limited to currently-advertised topics. Dropping a + // GenericSubscription genuinely stops DDS delivery — unsubscribed topics + // cost zero on the wire. + void reconcileSubscriptions() { + std::set desired; + for (const auto& [topic, type] : selected_topics_) { + desired.insert(topic); + } + for (const std::string& topic : host_active_) { + if (advertised_types_.count(topic) != 0) { + desired.insert(topic); + } + } + + // Concurrency contract to verify: reconcileSubscriptions() runs on the + // poll thread (called from onPoll() and onActiveTopicsChanged()), while + // executor_ spins concurrently on spinner_ (a MultiThreadedExecutor). + // Erasing a subscriptions_ entry here destroys the shared_ptr< + // GenericSubscription> — possibly the last reference — which tears down + // the underlying rcl subscription and its DDS entity while the executor + // may be mid-spin. This relies on rclcpp's guard-condition handling of + // dynamic entity removal (the executor's wait-set is expected to be + // notified and rebuilt safely without a data race or use-after-free on + // the subscription being spun). Flagged for per-distro stress + // verification (Humble/Iron/Jazzy) rather than assumed correct here. + for (auto it = subscriptions_.begin(); it != subscriptions_.end();) { + if (desired.count(it->first) == 0) { + it = subscriptions_.erase(it); + } else { + ++it; + } + } + for (const std::string& topic : desired) { + if (subscriptions_.count(topic) != 0) { + continue; + } + const std::string type = typeFor(topic); + if (type.empty()) { + continue; // Not currently advertised — subscribes when it appears. + } + subscribeTopic(topic, type); + } + } + + PJ::ParserBindingHandle* ensureBinding(const std::string& topic) { + auto it = binding_cache_.find(topic); + if (it != binding_cache_.end()) { + return &it->second; + } + + const std::string type_name = typeFor(topic); + if (type_name.empty()) { + return nullptr; + } + + const std::string* schema = schemaFor(type_name); + if (schema == nullptr) { + return nullptr; + } auto binding = runtimeHost().ensureParserBinding({ .topic_name = topic, .parser_encoding = "ros2msg", .type_name = type_name, - .schema = PJ::Span(reinterpret_cast(schema.data()), schema.size()), - .parser_config_json = parser_config_str, + .schema = PJ::Span(reinterpret_cast(schema->data()), schema->size()), + .parser_config_json = buildParserConfigString(), }); if (!binding) { runtimeHost().reportMessage( @@ -284,6 +453,11 @@ class Ros2StreamSource : public PJ::StreamSourceBase { context_.reset(); } binding_cache_.clear(); + advertised_types_.clear(); + schema_cache_.clear(); + host_active_.clear(); + lazy_mode_ = false; + discovery_tick_ = 0; { std::lock_guard lock(queue_mutex_); std::queue empty; @@ -291,10 +465,20 @@ class Ros2StreamSource : public PJ::StreamSourceBase { } } + // ~1 s at the host's 50 ms poll cadence — matches the dialog's discovery rate. + static constexpr int kDiscoveryPollTicks = 20; + Ros2Dialog dialog_; std::vector> selected_topics_; std::string parser_config_json_; + // Lazy subscription state — poll/start thread only (extension contract). + std::map advertised_types_; // topic -> first advertised type + std::unordered_map schema_cache_; // type -> schema ("" = unbuildable) + std::set host_active_; + bool lazy_mode_ = false; + int discovery_tick_ = 0; + std::shared_ptr context_; std::shared_ptr node_; std::unique_ptr executor_; diff --git a/data_stream_ros2/distro/src/ros2_dialog.hpp b/data_stream_ros2/distro/src/ros2_dialog.hpp index dfd7dadc..22e57b3f 100644 --- a/data_stream_ros2/distro/src/ros2_dialog.hpp +++ b/data_stream_ros2/distro/src/ros2_dialog.hpp @@ -91,7 +91,11 @@ class Ros2Dialog : public PJ::DialogPluginTyped { // companion deselect-all shortcut to the button (PJ3 parity). wd.setShortcut("btnDeselectAll", "Ctrl+Shift+A"); - wd.setOkEnabled(!selected_topics_.empty()); + // Selecting topics is optional: selected ones are always subscribed (the + // eager floor); on a lazy-subscription host the rest of the graph + // subscribes on demand as curves are dropped. On an older host the + // source still errors out when the selection is empty. + wd.setOkEnabled(true); return wd.toJson(); } diff --git a/extern/plotjuggler_core b/extern/plotjuggler_core index feba626e..6b558aff 160000 --- a/extern/plotjuggler_core +++ b/extern/plotjuggler_core @@ -1 +1 @@ -Subproject commit feba626e4c0462e50cd32c50ee2c532f05e68770 +Subproject commit 6b558aff4a4f51b88e2c6ace1d9fd76c60d43498 diff --git a/parser_ros/ros_parser.cpp b/parser_ros/ros_parser.cpp index 9e91dcae..af008fae 100644 --- a/parser_ros/ros_parser.cpp +++ b/parser_ros/ros_parser.cpp @@ -459,6 +459,135 @@ PJ::Status RosParser::compileBoundSchema(bool register_specialized_handler) { return PJ::okStatus(); } +namespace { + +// The subset of schema leaves whose flattened columns are fully determined by +// the schema alone, typed exactly as variantToValueRef will emit them. Arrays +// (fixed AND dynamic) are omitted — their columns appear on first data, which +// the manifest contract allows. Constants never flatten. +// +// `include_strings` gates BT::STRING leaves: when either string-to-number +// toggle (boolean_strings_to_number_ / remove_suffix_from_strings_) is on, +// flattenGeneric's parse()-time decision to emit a STRING field as float64 +// (or leave it as a string) depends on each value, not the schema alone — so +// the column's type cannot be pre-declared and the column must be omitted +// from the manifest entirely (it still materializes correctly on first data). +void collectManifestColumns( + const RosMsgParser::FieldTreeNode* node, const std::string& prefix, bool include_strings, + std::vector& out) { + using BT = RosMsgParser::BuiltinType; + for (const auto& child : node->children()) { + const RosMsgParser::ROSField* field = child.value(); + if (field == nullptr || field->isConstant() || field->isArray()) { + continue; + } + // Topic-relative path: no leading '/' for a top-level field, so it matches the + // flat field name live ingest (flattenGeneric/variantToValueRef) produces — the + // datastore's ensureField() strips a leading '/' from a manifest-supplied path, + // so a top-level field emitted here as "/data" would pre-create an orphan + // column distinct from the "data" column live data actually lands on. + const std::string path = prefix.empty() ? field->name() : prefix + "/" + field->name(); + const BT type_id = field->type().typeID(); + if (type_id == BT::OTHER) { + collectManifestColumns(&child, path, include_strings, out); + continue; + } + if (type_id == BT::STRING && !include_strings) { + continue; + } + PJ::PrimitiveType column_type = PJ::PrimitiveType::kFloat64; + switch (type_id) { + case BT::BOOL: + column_type = PJ::PrimitiveType::kBool; + break; + case BT::CHAR: + case BT::INT8: + column_type = PJ::PrimitiveType::kInt8; + break; + case BT::BYTE: + case BT::UINT8: + column_type = PJ::PrimitiveType::kUint8; + break; + case BT::INT16: + column_type = PJ::PrimitiveType::kInt16; + break; + case BT::UINT16: + column_type = PJ::PrimitiveType::kUint16; + break; + case BT::INT32: + column_type = PJ::PrimitiveType::kInt32; + break; + case BT::UINT32: + column_type = PJ::PrimitiveType::kUint32; + break; + case BT::INT64: + column_type = PJ::PrimitiveType::kInt64; + break; + case BT::UINT64: + column_type = PJ::PrimitiveType::kUint64; + break; + case BT::FLOAT32: + column_type = PJ::PrimitiveType::kFloat32; + break; + case BT::FLOAT64: + case BT::TIME: // variantToValueRef converts to double + case BT::DURATION: // variantToValueRef converts to double + column_type = PJ::PrimitiveType::kFloat64; + break; + case BT::STRING: + column_type = PJ::PrimitiveType::kString; + break; + default: + continue; // Unknown builtin — leave it to first-data discovery. + } + out.push_back({path, column_type}); + } +} + +} // namespace + +PJ::Expected> RosParser::describeSchemaColumns( + std::string_view type_name, PJ::Span schema) const { + // Only .msg-style generic flattening is pre-describable; specialized + // builtin-object handlers (images, pointclouds, markers) produce their + // scalar side dynamically and their topics are visible as object topics + // anyway. classifySchema-style: query the catalog and stand down for any + // schema with a specialized scalar handler. + const std::string msg_type = normalizedMessageType(type_name, schema_format_); + // Stand down for any msg_type that is itself a catalog key (specialized + // scalar/object handler picked purely by type), AND for any msg_type whose + // selectCatalogEntry() resolution differs from the kDefault (generic + // flatten) entry — this also covers the topic-conditional override (e.g. + // std_msgs/String on a robot_description topic), which is not a catalog + // key by itself but resolves to an object-only entry with no parse_scalars, + // so it emits no scalars and the manifest must stand down too. + const CatalogEntry selected_entry = selectCatalogEntry(msg_type); + const CatalogEntry& default_entry = catalog().at(CatalogEntry::kDefault); + if (catalog().find(msg_type) != catalog().end() || selected_entry.parse_scalars != default_entry.parse_scalars) { + return PJ::unexpected(std::string("specialized handler — no static column manifest for ") + msg_type); + } + + const std::string schema_str(reinterpret_cast(schema.data()), schema.size()); + std::optional schema_parser; + try { + schema_parser.emplace("", RosMsgParser::ROSType(msg_type), schema_str, schema_format_); + } catch (const std::exception& e) { + return PJ::unexpected(std::string("cannot pre-describe schema for ") + msg_type + ": " + e.what()); + } + + std::vector columns; + const auto& message_schema = schema_parser->getSchema(); + if (message_schema == nullptr || message_schema->field_tree.croot() == nullptr) { + return PJ::unexpected(std::string("empty schema tree for ") + msg_type); + } + // See the comment on collectManifestColumns: when a string-to-number + // toggle is enabled, a STRING leaf's manifest type is value-dependent and + // therefore cannot be pre-declared, so string columns are omitted. + const bool include_strings = !(boolean_strings_to_number_ || remove_suffix_from_strings_); + collectManifestColumns(message_schema->field_tree.croot(), "", include_strings, columns); + return columns; +} + std::string RosParser::saveConfig() const { nlohmann::json cfg; pj::array_policy::arrayLimitToJson(cfg, static_cast(max_array_size_), !discard_large_arrays_); diff --git a/parser_ros/ros_parser_internal.hpp b/parser_ros/ros_parser_internal.hpp index c6151d62..8ad4ffbd 100644 --- a/parser_ros/ros_parser_internal.hpp +++ b/parser_ros/ros_parser_internal.hpp @@ -159,6 +159,8 @@ class RosParser : public PJ::MessageParserPluginBase { std::string saveConfig() const override; PJ::Status loadConfig(std::string_view config_json) override; PJ::Status parse(PJ::Timestamp timestamp_ns, PJ::Span payload) override; + PJ::Expected> describeSchemaColumns( + std::string_view type_name, PJ::Span schema) const override; // ----- Class-level schema catalog ----- // diff --git a/parser_ros/tests/ros_parser_test.cpp b/parser_ros/tests/ros_parser_test.cpp index 177f8850..1070a705 100644 --- a/parser_ros/tests/ros_parser_test.cpp +++ b/parser_ros/tests/ros_parser_test.cpp @@ -84,6 +84,32 @@ std::vector serializeCdr(const std::function(encoder.getBufferData(), encoder.getBufferData() + encoder.getBufferSize()); } +// Mirrors PJ::normalizeFieldPath (pj_datastore/include/pj_datastore/plugin_data_host.hpp): +// the datastore's ensureField() collapses internal "//" runs to "/" and drops a single +// leading '/' before using a field name as a storage key. RosMsgParser's own flat-key +// convention (used by the LIVE parse path, e.g. RosParser::flattenGeneric) always +// prefixes a top-level field with '/' (its root node's cached path is the empty schema +// "topic name"), so a raw parseScalars-emitted name and a topic-relative manifest path +// (no leading slash, see collectManifestColumns) only agree once both go through this +// normalization — exactly as they do at the real ensureField() call site. parser_ros +// has no dependency on pj_datastore, hence this small parallel implementation for tests. +std::string normalizeLikeDatastore(std::string_view name) { + std::string out; + out.reserve(name.size()); + bool prev_slash = false; + for (const char c : name) { + if (c == '/' && prev_slash) { + continue; + } + prev_slash = (c == '/'); + out.push_back(c); + } + if (!out.empty() && out.front() == '/') { + out.erase(0, 1); + } + return out; +} + // --- ROS message definitions (text format) --- // Simple scalar message: int32 + float64 + bool @@ -167,6 +193,163 @@ TEST(RosParserTest, SimpleScalarMessage) { EXPECT_TRUE(found_active); } +TEST(RosParserTest, DescribeSchemaColumnsMatchesParseEmission) { + RosParserFixture f; + f.setUp(); + ASSERT_TRUE(f.bindSchema("pkg/Nested", kNestedDef)); + + const std::string def(kNestedDef); + std::string manifest_json; + auto status = f.handle.describeSchemaColumns( + "pkg/Nested", PJ::Span(reinterpret_cast(def.data()), def.size()), manifest_json); + ASSERT_TRUE(status) << status.error(); + + auto manifest = nlohmann::json::parse(manifest_json); + ASSERT_TRUE(manifest.is_array()); + ASSERT_FALSE(manifest.empty()); + + // Parse one fully-populated message and compare STRICTLY: every manifest + // column must be emitted with the same name and type. (The reverse need + // not hold — arrays and derived RPY columns are legitimately absent from + // the manifest and appear on first data.) + auto payload = serializeCdr([](RosMsgParser::NanoCDR_Serializer& enc) { + enc.serialize(RosMsgParser::INT32, RosMsgParser::Variant(static_cast(1234))); + enc.serialize(RosMsgParser::UINT32, RosMsgParser::Variant(static_cast(567))); + enc.serializeString("map"); + enc.serialize(RosMsgParser::FLOAT64, RosMsgParser::Variant(1.5)); + }); + ASSERT_TRUE(f.parse(payload)); + ASSERT_EQ(f.recorder.rows().size(), 1u); + const auto& row = f.recorder.rows()[0]; + + for (const auto& entry : manifest) { + const auto path = entry["path"].get(); + // Manifest paths are topic-relative: never a leading '/', even for a + // top-level field (see collectManifestColumns). This is what makes the + // pre-created column agree with the live column ensureField() lands + // data on, closing the lazy-subscription orphan-column bug. + ASSERT_FALSE(path.empty()); + EXPECT_NE(path.front(), '/') << "manifest path must be topic-relative (no leading '/'): " << path; + const auto type = PJ::sdk::primitiveTypeFromJsonName(entry["type"].get()); + ASSERT_TRUE(type.has_value()) << path; + // The raw field name parseScalars/append_record hands the write host may + // still carry RosMsgParser's own leading '/' convention (see + // normalizeLikeDatastore) — ensureField() normalizes it the same way + // before it becomes a storage key, so comparing post-normalization is + // what actually proves "same field, no duplicate column", matching what + // the real datastore does. + const PJ::sdk::testing::RecordedField* field = nullptr; + for (const auto& candidate : row.fields) { + if (normalizeLikeDatastore(candidate.name) == path) { + field = &candidate; + break; + } + } + ASSERT_NE(field, nullptr) << "manifest column not emitted by parse(): " << path; + EXPECT_EQ(field->type, *type) << path; + } +} + +TEST(RosParserTest, DescribeSchemaColumnsTopLevelFieldMatchesLiveFieldAfterNormalization) { + // Regression test for the lazy-subscription orphan-column bug: a manifest + // column for a top-level scalar field (e.g. std_msgs/Float64's "data") must + // resolve to the SAME datastore key as the live-ingested field, not a + // second, never-updated column. collectManifestColumns emits a bare + // top-level path ("data", no leading slash); RosMsgParser's own live parse + // path emits "/data" (its flat-key convention); both must normalize + // (normalizeLikeDatastore / PJ::normalizeFieldPath) to the identical key. + RosParserFixture f; + f.setUp(); + const std::string def = "float64 data\n"; + ASSERT_TRUE(f.bindSchema("std_msgs/Float64", def)); + + std::string manifest_json; + auto status = f.handle.describeSchemaColumns( + "std_msgs/Float64", PJ::Span(reinterpret_cast(def.data()), def.size()), + manifest_json); + ASSERT_TRUE(status) << status.error(); + + auto manifest = nlohmann::json::parse(manifest_json); + ASSERT_TRUE(manifest.is_array()); + ASSERT_EQ(manifest.size(), 1u); + EXPECT_EQ(manifest[0]["path"].get(), "data"); + + auto payload = serializeCdr( + [](RosMsgParser::NanoCDR_Serializer& enc) { enc.serialize(RosMsgParser::FLOAT64, RosMsgParser::Variant(2.5)); }); + ASSERT_TRUE(f.parse(payload)); + ASSERT_EQ(f.recorder.rows().size(), 1u); + ASSERT_EQ(f.recorder.rows()[0].fields.size(), 1u); + + const auto& live_field = f.recorder.rows()[0].fields[0]; + EXPECT_EQ(live_field.name, "/data") << "sanity check on RosMsgParser's own flat-key convention"; + EXPECT_EQ(normalizeLikeDatastore(live_field.name), manifest[0]["path"].get()) + << "manifest key and normalized live key must be exactly the same field"; +} + +TEST(RosParserTest, DescribeSchemaColumnsStandsDownForSpecializedSchemas) { + RosParserFixture f; + f.setUp(); + const std::string def = "byte[] data\n"; + ASSERT_TRUE(f.bindSchema("sensor_msgs/CompressedImage", def)); + std::string manifest_json; + auto status = f.handle.describeSchemaColumns( + "sensor_msgs/CompressedImage", PJ::Span(reinterpret_cast(def.data()), def.size()), + manifest_json); + EXPECT_FALSE(status); +} + +TEST(RosParserTest, DescribeSchemaColumnsOmitsStringsWhenStringToNumberToggleOn) { + // With either string-to-number toggle enabled, parse() may emit a STRING + // field as float64 depending on the runtime value — so its manifest type + // can't be pre-declared, and the column must be omitted entirely rather + // than published with a possibly-wrong type. Numeric-only siblings must + // still be present. + RosParserFixture f; + f.setUp(); + ASSERT_TRUE(f.bindSchema("pkg/Nested", kNestedDef)); + ASSERT_TRUE(f.handle.loadConfig(R"({"boolean_strings_to_number":true})")); + + const std::string def(kNestedDef); + std::string manifest_json; + auto status = f.handle.describeSchemaColumns( + "pkg/Nested", PJ::Span(reinterpret_cast(def.data()), def.size()), manifest_json); + ASSERT_TRUE(status) << status.error(); + + auto manifest = nlohmann::json::parse(manifest_json); + ASSERT_TRUE(manifest.is_array()); + + bool found_frame_id = false; + bool found_value = false; + for (const auto& entry : manifest) { + const auto path = entry["path"].get(); + if (path == "header/frame_id") { + found_frame_id = true; + } else if (path == "value") { + found_value = true; + } + } + EXPECT_FALSE(found_frame_id) << "value-dependent string column must be omitted from the manifest"; + EXPECT_TRUE(found_value) << "numeric column must still be present"; +} + +TEST(RosParserTest, DescribeSchemaColumnsStandsDownForRobotDescriptionTopic) { + // std_msgs/String isn't a catalog key by itself, but on a robot_description + // topic selectCatalogEntry() overrides it to an object-only entry with no + // parse_scalars — no scalars are ever emitted, so the manifest must stand + // down too, not just publish an empty/misleading column list. + RosParserFixture f; + f.setUp(); + ASSERT_TRUE(f.handle.loadConfig(R"({"topic_name":"robot_description"})")); + ASSERT_TRUE(f.bindSchema("std_msgs/String", kStringDef)); + + const std::string def(kStringDef); + std::string manifest_json; + auto status = f.handle.describeSchemaColumns( + "std_msgs/String", PJ::Span(reinterpret_cast(def.data()), def.size()), + manifest_json); + EXPECT_FALSE(status); +} + TEST(RosParserTest, NestedMessage) { RosParserFixture f; f.setUp(); diff --git a/tools/anomaly_runner/CMakeUserPresets.json b/tools/anomaly_runner/CMakeUserPresets.json new file mode 100644 index 00000000..f544ebe0 --- /dev/null +++ b/tools/anomaly_runner/CMakeUserPresets.json @@ -0,0 +1,9 @@ +{ + "version": 4, + "vendor": { + "conan": {} + }, + "include": [ + "../../build/tools/anomaly_runner/CMakePresets.json" + ] +}