From 81d0af8bcbd84148efdec2f2fd9f5b27503352c5 Mon Sep 17 00:00:00 2001 From: Khuzaymah Bin Haris Date: Tue, 4 Aug 2026 04:40:51 +0300 Subject: [PATCH] Make diagnostics mock/live source handoff thread safe Switching the DiagnosticsPanel back to mock destroyed the live RosDiagnosticsSource from the Qt thread while a ROS executor thread could still be inside its subscription callback, because the callback captured a raw `this`. Received state now lives in a separately owned block that the callback captures by shared_ptr, so a callback can never dereference a destroyed source. DiagnosticsSource gains a stop() hook that RosDiagnosticsSource uses to drop the subscription and retire its state under the mutex, and the panel retires the outgoing source before installing its replacement. The refresh timer pins its source for the duration of a tick so it cannot observe a half-replaced source. Adds a gtest stress suite that churns live sources under multi-threaded traffic. Mutating the source back to capturing `this` makes the suite report a heap-use-after-free under AddressSanitizer, confirming it detects the original defect. Co-authored-by: Cursor --- .gitignore | 4 + waybionic_rviz_plugins/CMakeLists.txt | 18 + waybionic_rviz_plugins/README.md | 5 + .../docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md | 128 +++++++ .../diagnostics_panel.hpp | 12 +- .../diagnostics_source.hpp | 4 + .../ros_diagnostics_source.hpp | 45 ++- waybionic_rviz_plugins/package.xml | 1 + .../src/diagnostics_panel.cpp | 54 ++- .../src/ros_diagnostics_source.cpp | 155 +++++---- .../test/test_ros_diagnostics_source.cpp | 313 ++++++++++++++++++ 11 files changed, 659 insertions(+), 80 deletions(-) create mode 100644 waybionic_rviz_plugins/docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md create mode 100644 waybionic_rviz_plugins/test/test_ros_diagnostics_source.cpp diff --git a/.gitignore b/.gitignore index 28b0c9f..de74c75 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,10 @@ build/ install/ log/ +# Python caches +__pycache__/ +*.py[cod] + # imported ROS projects easy_handeye2/ ros2_aruco/ diff --git a/waybionic_rviz_plugins/CMakeLists.txt b/waybionic_rviz_plugins/CMakeLists.txt index 2d8ec7f..ade07c3 100644 --- a/waybionic_rviz_plugins/CMakeLists.txt +++ b/waybionic_rviz_plugins/CMakeLists.txt @@ -72,6 +72,7 @@ install( ) if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_lint_cmake REQUIRED) find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_xmllint REQUIRED) @@ -84,6 +85,23 @@ if(BUILD_TESTING) test/test_package_metadata.py TIMEOUT 60 ) + + # Built from sources directly so the stress test links neither Qt nor RViz and + # can run headless. + ament_add_gtest(test_ros_diagnostics_source + test/test_ros_diagnostics_source.cpp + src/ros_diagnostics_source.cpp + TIMEOUT 300 + ) + if(TARGET test_ros_diagnostics_source) + target_include_directories(test_ros_diagnostics_source PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + ament_target_dependencies(test_ros_diagnostics_source + diagnostic_msgs + rclcpp + ) + endif() endif() ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/waybionic_rviz_plugins/README.md b/waybionic_rviz_plugins/README.md index e1d5fd5..18eb62c 100644 --- a/waybionic_rviz_plugins/README.md +++ b/waybionic_rviz_plugins/README.md @@ -55,8 +55,10 @@ waybionic_rviz_plugins/ temporary_diagnostics_publisher.launch.py test/ test_package_metadata.py + test_ros_diagnostics_source.cpp # Live/mock source handoff stress tests docs/ DIAGNOSTICS_CONTRACT.md + DIAGNOSTICS_SOURCE_LIFECYCLE.md GROUND_STATION_RVIZ_UI.md PR_NOTES.md ``` @@ -137,6 +139,8 @@ DiagnosticsSource `RosDiagnosticsSource` maps ROS diagnostic levels and fields into the internal `DiagnosticMessage` model before the Qt panel renders them. See `docs/DIAGNOSTICS_CONTRACT.md` for the full mapping Korede/backend should follow, and `docs/DIAGNOSTICS_BACKEND_INTEGRATION.md` for backend replacement guidance. +Switching between mock and live replaces the active source while a ROS callback may still be running. `docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md` documents the ownership rules that keep that handoff safe and the stress test that guards it. + ## Platform Notes - Primary validation target is Ubuntu/WSL2 with ROS 2 Jazzy. @@ -146,6 +150,7 @@ DiagnosticsSource - `docs/DIAGNOSTICS_CONTRACT.md` — normalized diagnostic model and ROS mapping - `docs/DIAGNOSTICS_BACKEND_INTEGRATION.md` — how a real backend replaces the temporary publisher +- `docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md` — mock/live source ownership and the handoff stress test - `docs/GROUND_STATION_RVIZ_UI.md` — extended architecture notes - `docs/PR_NOTES.md` — review summary and PR description source diff --git a/waybionic_rviz_plugins/docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md b/waybionic_rviz_plugins/docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md new file mode 100644 index 0000000..5d51a64 --- /dev/null +++ b/waybionic_rviz_plugins/docs/DIAGNOSTICS_SOURCE_LIFECYCLE.md @@ -0,0 +1,128 @@ +# Diagnostics Source Lifecycle + +`DiagnosticsPanel` can swap its data source at runtime when the operator toggles +**Use Mock Diagnostics**, or when launch parameters are applied after RViz +restores a saved config. This note documents the ownership rules that make that +swap safe, because the source is read by the Qt thread and written by a ROS +executor thread at the same time. + +## The problem + +`RosDiagnosticsSource` owns a subscription to a `diagnostic_msgs/msg/DiagnosticArray` +topic. Two threads are involved: + +| Thread | Work | +|--------|------| +| ROS executor | Runs the subscription callback and writes received diagnostics | +| Qt main thread | Runs the 1 Hz refresh timer, reads diagnostics, and swaps sources | + +Switching from live back to mock destroys the live source from the Qt thread. If +the subscription callback holds a raw pointer to that object, the callback can be +executing on the executor thread at the moment the object is freed, which is a +use-after-free. + +```mermaid +sequenceDiagram + participant Qt as Qt thread + participant Exec as ROS executor + participant Src as RosDiagnosticsSource + Qt->>Src: configureSource(mock) + Exec->>Src: diagnosticsCallback() running + Qt->>Src: destroy + Exec-->>Src: writes into freed memory +``` + +## Ownership rules + +1. **Received state is owned separately from the source object.** + `RosDiagnosticsSource` keeps its messages in a `SharedState` block held by + `shared_ptr`. The subscription callback captures that `shared_ptr` by value + instead of capturing `this`, so the callback never dereferences the source + object and cannot outlive its own data. + +2. **Retirement is explicit.** `DiagnosticsSource::stop()` is a virtual no-op that + `RosDiagnosticsSource` overrides. It drops the subscription handle and then + sets `active = false` under the state mutex. A callback already mid-write + finishes first; any later dispatch of a queued message sees `active == false` + and returns without touching the state. + +3. **The panel retires before it replaces.** `DiagnosticsPanel::configureSource()` + moves the outgoing source into a local variable, calls `stop()` on it, installs + the replacement, and only then releases the retired source. This also + guarantees a second subscription is never created while the first is still + attached. + +4. **The refresh timer pins its source.** `DiagnosticsPanel::refresh()` copies the + `shared_ptr` for the duration of the tick, so a swap mid-tick cannot leave the + widgets reading a half-replaced source. + +5. **The panel destructor stops the timer first**, then stops the source, so no + callback or timer fires against partly destroyed members. + +The UI thread is never blocked for longer than one vector assignment, and no +source is leaked: `stop()` is idempotent and the destructor calls it. + +## Repeatable stress check + +`test/test_ros_diagnostics_source.cpp` is the automated equivalent of toggling +the checkbox repeatedly with the cycle publisher running. It spins a +`MultiThreadedExecutor` with four threads while a separate thread publishes on +the topic every 200 microseconds, then creates, reads, stops, and destroys live +sources in a loop. + +| Test | What it proves | +|------|----------------| +| `ReportsWaitingBeforeAnyMessageArrives` | Live mode reports a waiting row before traffic | +| `NormalizesReceivedDiagnostics` | Received statuses map onto the internal contract | +| `StopFreezesStateAndIgnoresLaterMessages` | A retired source stops ingesting messages | +| `StopIsIdempotent` | Repeated `stop()` calls and later reads are safe | +| `RepeatedLiveMockChurnUnderTrafficIsSafe` | 300 create/stop/destroy cycles under load | +| `DestructionWithoutExplicitStopIsSafe` | The destructor alone retires the subscription | +| `ConcurrentReadsDuringTeardownAreSafe` | A reader thread racing `stop()` is safe | +| `ChurnLeavesNoLingeringSubscription` | No subscription leak after repeated churn | + +Run it with: + +```bash +source /opt/ros/jazzy/setup.bash +colcon build --packages-select waybionic_rviz_plugins --symlink-install +colcon test --packages-select waybionic_rviz_plugins +colcon test-result --all --verbose +``` + +### Confirming the stress check actually detects the race + +A stress test that passes against both the broken and the fixed code proves +nothing, so the suite was validated by mutation under AddressSanitizer. The +stress test and `src/ros_diagnostics_source.cpp` were compiled standalone twice: +once against the current source, and once against a mutated copy whose +subscription callback captures `this` and whose `stop()` does not drop the +subscription, which is the ownership model that existed before this change. + +| Build | Result | +|-------|--------| +| Current source | 4 stress tests pass | +| Mutated to capture `this` | `AddressSanitizer: heap-use-after-free` in `std::__shared_ptr::get()`, raised by `ConcurrentReadsDuringTeardownAreSafe` | + +`new_delete_type_mismatch` has to be disabled for these runs because rclcpp's +internal C allocator shim trips it during plain node construction, unrelated to +this code: + +```bash +ASAN_OPTIONS=detect_leaks=0:new_delete_type_mismatch=0 ./test_ros_diagnostics_source +``` + +### Manual GUI equivalent + +```bash +# Terminal 1 +ros2 launch waybionic_rviz_plugins temporary_diagnostics_publisher.launch.py mode:=cycle + +# Terminal 2 +ros2 launch waybionic_rviz_plugins engineer_view.launch.py use_mock_diagnostics:=false +``` + +Toggle **Use Mock Diagnostics** on and off repeatedly while messages arrive. Expected +behaviour: no crash or freeze, the source label alternates between `Mock` and +`ROS /diagnostics`, the mock buttons enable only in mock mode, and live rows resume +updating each time live mode is re-selected. diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp index 4433841..ab45aa1 100644 --- a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp @@ -33,6 +33,7 @@ class DiagnosticsPanel : public rviz_common::Panel public: explicit DiagnosticsPanel(QWidget * parent = nullptr); + ~DiagnosticsPanel() override; void onInitialize() override; void save(rviz_common::Config config) const override; @@ -46,7 +47,10 @@ class DiagnosticsPanel : public rviz_common::Panel void refresh(); void setMockDiagnosticsState(MockDiagnosticsState mode); void setUseMockDiagnostics(bool use_mock_diagnostics); - void updateSystemStatus(const std::vector & messages, const rclcpp::Time & now); + void updateSystemStatus( + const DiagnosticsSource & source, + const std::vector & messages, + const rclcpp::Time & now); void updateTelemetryTable(const std::vector & messages, const rclcpp::Time & now); void updateAlerts(const std::vector & messages); void updateSourceControls(); @@ -58,8 +62,10 @@ class DiagnosticsPanel : public rviz_common::Panel QString optionalText(const std::optional & value) const; QString alertText(const DiagnosticMessage & message) const; - std::unique_ptr diagnostics_source_; - MockDiagnosticsSource * mock_diagnostics_source_{nullptr}; + // Shared ownership so a refresh tick keeps its source alive even if a mode + // switch replaces the panel's source part-way through the tick. + std::shared_ptr diagnostics_source_; + std::shared_ptr mock_diagnostics_source_; rclcpp::Node::SharedPtr rviz_node_; rclcpp::Clock clock_{RCL_SYSTEM_TIME}; std::string diagnostics_topic_{"/diagnostics"}; diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp index 7114626..482d1e7 100644 --- a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_source.hpp @@ -16,6 +16,10 @@ class DiagnosticsSource public: virtual ~DiagnosticsSource() = default; + /// Detach any external data feed so no further callbacks mutate this source. + /// Must be idempotent and safe to call from the UI thread. + virtual void stop() {} + virtual std::string sourceName() const = 0; virtual std::string connectionStatus(const rclcpp::Time & now) const = 0; virtual std::vector messages(const rclcpp::Time & now) const = 0; diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp index 5101ba3..af10065 100644 --- a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/ros_diagnostics_source.hpp @@ -1,6 +1,7 @@ #ifndef WAYBIONIC_RVIZ_PLUGINS__ROS_DIAGNOSTICS_SOURCE_HPP_ #define WAYBIONIC_RVIZ_PLUGINS__ROS_DIAGNOSTICS_SOURCE_HPP_ +#include #include #include #include @@ -13,6 +14,13 @@ namespace waybionic_rviz_plugins { +/// Live diagnostics source backed by a ROS 2 subscription. +/// +/// The subscription callback runs on an executor thread while the RViz panel +/// reads and replaces sources from the Qt thread. Received data therefore lives +/// in a separately owned state block that the callback keeps alive, so tearing +/// this object down can never leave an in-flight callback writing into freed +/// memory. class RosDiagnosticsSource : public DiagnosticsSource { public: @@ -20,24 +28,43 @@ class RosDiagnosticsSource : public DiagnosticsSource rclcpp::Node::SharedPtr node, std::string diagnostics_topic = "/diagnostics"); + ~RosDiagnosticsSource() override; + + RosDiagnosticsSource(const RosDiagnosticsSource &) = delete; + RosDiagnosticsSource & operator=(const RosDiagnosticsSource &) = delete; + RosDiagnosticsSource(RosDiagnosticsSource &&) = delete; + RosDiagnosticsSource & operator=(RosDiagnosticsSource &&) = delete; + + /// Drops the subscription and retires the shared state. Any callback that is + /// already executing finishes against the retired state and is discarded. + void stop() override; + + bool isStopped() const; + std::string sourceName() const override; std::string connectionStatus(const rclcpp::Time & now) const override; std::vector messages(const rclcpp::Time & now) const override; private: - void diagnosticsCallback(diagnostic_msgs::msg::DiagnosticArray::SharedPtr message); - DiagnosticMessage toDiagnosticMessage( - const diagnostic_msgs::msg::DiagnosticStatus & status, - const rclcpp::Time & timestamp) const; + /// Data shared between the executor thread and the Qt thread. Held by + /// shared_ptr so the subscription callback always operates on live memory. + struct SharedState + { + mutable std::mutex mutex; + std::vector latest_messages; + rclcpp::Time last_received_time{0, 0, RCL_SYSTEM_TIME}; + bool has_received{false}; + bool active{true}; + }; + + static void handleMessage( + const std::shared_ptr & state, + const diagnostic_msgs::msg::DiagnosticArray & message); rclcpp::Node::SharedPtr node_; std::string diagnostics_topic_; rclcpp::Subscription::SharedPtr subscription_; - - mutable std::mutex mutex_; - std::vector latest_messages_; - rclcpp::Time last_received_time_{0, 0, RCL_SYSTEM_TIME}; - bool has_received_{false}; + std::shared_ptr state_; }; } // namespace waybionic_rviz_plugins diff --git a/waybionic_rviz_plugins/package.xml b/waybionic_rviz_plugins/package.xml index 04ed636..1b076cd 100644 --- a/waybionic_rviz_plugins/package.xml +++ b/waybionic_rviz_plugins/package.xml @@ -20,6 +20,7 @@ rclpy rviz2 + ament_cmake_gtest ament_cmake_lint_cmake ament_cmake_pytest ament_cmake_xmllint diff --git a/waybionic_rviz_plugins/src/diagnostics_panel.cpp b/waybionic_rviz_plugins/src/diagnostics_panel.cpp index 4f1e062..ac0fde3 100644 --- a/waybionic_rviz_plugins/src/diagnostics_panel.cpp +++ b/waybionic_rviz_plugins/src/diagnostics_panel.cpp @@ -125,6 +125,18 @@ DiagnosticsPanel::DiagnosticsPanel(QWidget * parent) configureSource(use_mock_diagnostics_); } +DiagnosticsPanel::~DiagnosticsPanel() +{ + // Stop the timer before the members it reads are destroyed, then detach the + // live subscription so no executor callback outlives the panel. + if (refresh_timer_ != nullptr) { + refresh_timer_->stop(); + } + if (diagnostics_source_) { + diagnostics_source_->stop(); + } +} + void DiagnosticsPanel::onInitialize() { if (auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock()) { @@ -275,17 +287,29 @@ void DiagnosticsPanel::buildUi() void DiagnosticsPanel::configureSource(const bool use_mock_diagnostics) { use_mock_diagnostics_ = use_mock_diagnostics; - mock_diagnostics_source_ = nullptr; + + // Detach the outgoing source before releasing it. Retiring the subscription + // first means an executor callback can no longer publish into a source the + // panel has stopped using, and it prevents a second subscription from + // existing alongside the old one. + auto retired_source = std::move(diagnostics_source_); + if (retired_source) { + retired_source->stop(); + } + mock_diagnostics_source_.reset(); if (use_mock_diagnostics_ || !rviz_node_) { - auto mock_source = std::make_unique(); - mock_diagnostics_source_ = mock_source.get(); - diagnostics_source_ = std::move(mock_source); + mock_diagnostics_source_ = std::make_shared(); + diagnostics_source_ = mock_diagnostics_source_; } else { - diagnostics_source_ = std::make_unique(rviz_node_, diagnostics_topic_); + diagnostics_source_ = std::make_shared(rviz_node_, diagnostics_topic_); } updateSourceControls(); + + // Released only after the replacement is installed, so no refresh can observe + // a panel without a source. + retired_source.reset(); } bool DiagnosticsPanel::readUseMockDiagnosticsParameter(const bool default_value) @@ -316,16 +340,23 @@ std::string DiagnosticsPanel::readDiagnosticsTopicParameter(const std::string & void DiagnosticsPanel::refresh() { + // Pin the source for the whole tick so a mode switch cannot swap it out + // between reading the messages and rendering them. + const auto source = diagnostics_source_; + if (!source) { + return; + } + const auto now = clock_.now(); - const auto messages = diagnostics_source_->messages(now); - updateSystemStatus(messages, now); + const auto messages = source->messages(now); + updateSystemStatus(*source, messages, now); updateTelemetryTable(messages, now); updateAlerts(messages); } void DiagnosticsPanel::setMockDiagnosticsState(const MockDiagnosticsState mode) { - if (mock_diagnostics_source_ == nullptr) { + if (!mock_diagnostics_source_) { return; } @@ -340,6 +371,7 @@ void DiagnosticsPanel::setUseMockDiagnostics(const bool use_mock_diagnostics) } void DiagnosticsPanel::updateSystemStatus( + const DiagnosticsSource & source, const std::vector & messages, const rclcpp::Time & now) { @@ -364,8 +396,8 @@ void DiagnosticsPanel::updateSystemStatus( state_label_->style()->unpolish(state_label_); state_label_->style()->polish(state_label_); - source_label_->setText(QString::fromStdString(diagnostics_source_->sourceName())); - ros_connection_label_->setText(QString::fromStdString(diagnostics_source_->connectionStatus(now))); + source_label_->setText(QString::fromStdString(source.sourceName())); + ros_connection_label_->setText(QString::fromStdString(source.connectionStatus(now))); heartbeat_label_->setText(has_stale ? "STALE" : "OK"); heartbeat_label_->setStyleSheet(QString("color: %1; font-weight: 800;").arg(has_stale ? "#9aa4ad" : "#3ddc84")); safety_label_->setStyleSheet(QString("color: %1; font-weight: 700;").arg(has_alert ? "#ff4d5e" : "#8ea3b1")); @@ -389,7 +421,7 @@ void DiagnosticsPanel::updateSourceControls() use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); } - const bool mock_enabled = mock_diagnostics_source_ != nullptr; + const bool mock_enabled = static_cast(mock_diagnostics_source_); if (normal_button_ != nullptr) { normal_button_->setEnabled(mock_enabled); if (!mock_enabled) { diff --git a/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp b/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp index a16898a..33a3387 100644 --- a/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp +++ b/waybionic_rviz_plugins/src/ros_diagnostics_source.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -45,22 +46,97 @@ bool hasContent(const std::string & value) return !value.empty(); } +DiagnosticMessage toDiagnosticMessage( + const diagnostic_msgs::msg::DiagnosticStatus & status, + const rclcpp::Time & timestamp) +{ + std::optional value; + std::optional unit; + + for (const auto & key_value : status.values) { + const auto key = lowerCopy(key_value.key); + if (key == "value") { + value = key_value.value; + continue; + } + if (key == "unit") { + unit = key_value.value; + continue; + } + if (!value.has_value() && hasContent(key_value.value)) { + value = key_value.value; + unit = key_value.key; + } + } + + const auto normalized_status = mapLevel(status.level); + std::optional alert_message; + if (hasContent(status.message) && normalized_status != DiagnosticStatus::Ok) { + alert_message = status.message; + } + + return { + status.name, + normalized_status, + timestamp, + value, + unit, + alert_message, + }; +} + } // namespace RosDiagnosticsSource::RosDiagnosticsSource( rclcpp::Node::SharedPtr node, std::string diagnostics_topic) : node_(std::move(node)), - diagnostics_topic_(std::move(diagnostics_topic)) + diagnostics_topic_(std::move(diagnostics_topic)), + state_(std::make_shared()) { + // The callback captures the state by value rather than `this`, so the source + // object can be destroyed while a callback is still running. + auto state = state_; subscription_ = node_->create_subscription( diagnostics_topic_, rclcpp::QoS(10), - [this](diagnostic_msgs::msg::DiagnosticArray::SharedPtr message) { - diagnosticsCallback(std::move(message)); + [state](diagnostic_msgs::msg::DiagnosticArray::SharedPtr message) { + handleMessage(state, *message); }); } +RosDiagnosticsSource::~RosDiagnosticsSource() +{ + stop(); +} + +void RosDiagnosticsSource::stop() +{ + // Dropping our handle removes the subscription from the executor. rclcpp keeps + // the underlying subscription alive for the duration of a callback that has + // already been dispatched, so this never tears out running work. + subscription_.reset(); + + if (!state_) { + return; + } + + // Retiring under the lock means a callback that is mid-write completes first, + // and any later dispatch of an already-queued message becomes a no-op. The + // wait is bounded by one vector assignment, so the UI thread is not blocked. + std::lock_guard lock(state_->mutex); + state_->active = false; +} + +bool RosDiagnosticsSource::isStopped() const +{ + if (!state_) { + return true; + } + std::lock_guard lock(state_->mutex); + return !state_->active; +} + std::string RosDiagnosticsSource::sourceName() const { return "ROS " + diagnostics_topic_; @@ -68,12 +144,12 @@ std::string RosDiagnosticsSource::sourceName() const std::string RosDiagnosticsSource::connectionStatus(const rclcpp::Time & now) const { - std::lock_guard lock(mutex_); - if (!has_received_) { + std::lock_guard lock(state_->mutex); + if (!state_->has_received) { return "Waiting for " + diagnostics_topic_; } - const double age_seconds = std::max(0.0, (now - last_received_time_).seconds()); + const double age_seconds = std::max(0.0, (now - state_->last_received_time).seconds()); if (age_seconds > kStaleAfterSeconds) { return "No recent messages on " + diagnostics_topic_; } @@ -83,8 +159,8 @@ std::string RosDiagnosticsSource::connectionStatus(const rclcpp::Time & now) con std::vector RosDiagnosticsSource::messages(const rclcpp::Time & now) const { - std::lock_guard lock(mutex_); - if (!has_received_) { + std::lock_guard lock(state_->mutex); + if (!state_->has_received) { return {{ "diagnostics.topic", DiagnosticStatus::Stale, @@ -95,8 +171,8 @@ std::vector RosDiagnosticsSource::messages(const rclcpp::Time }}; } - auto messages = latest_messages_; - const double age_seconds = std::max(0.0, (now - last_received_time_).seconds()); + auto messages = state_->latest_messages; + const double age_seconds = std::max(0.0, (now - state_->last_received_time).seconds()); if (age_seconds <= kStaleAfterSeconds) { return messages; } @@ -110,65 +186,30 @@ std::vector RosDiagnosticsSource::messages(const rclcpp::Time return messages; } -void RosDiagnosticsSource::diagnosticsCallback( - diagnostic_msgs::msg::DiagnosticArray::SharedPtr message) +void RosDiagnosticsSource::handleMessage( + const std::shared_ptr & state, + const diagnostic_msgs::msg::DiagnosticArray & message) { rclcpp::Clock system_clock(RCL_SYSTEM_TIME); const auto received_at = system_clock.now(); - rclcpp::Time timestamp(message->header.stamp, RCL_SYSTEM_TIME); + rclcpp::Time timestamp(message.header.stamp, RCL_SYSTEM_TIME); if (timestamp.nanoseconds() == 0) { timestamp = received_at; } std::vector normalized_messages; - normalized_messages.reserve(message->status.size()); - for (const auto & status : message->status) { + normalized_messages.reserve(message.status.size()); + for (const auto & status : message.status) { normalized_messages.push_back(toDiagnosticMessage(status, timestamp)); } - std::lock_guard lock(mutex_); - latest_messages_ = std::move(normalized_messages); - last_received_time_ = received_at; - has_received_ = true; -} - -DiagnosticMessage RosDiagnosticsSource::toDiagnosticMessage( - const diagnostic_msgs::msg::DiagnosticStatus & status, - const rclcpp::Time & timestamp) const -{ - std::optional value; - std::optional unit; - - for (const auto & key_value : status.values) { - const auto key = lowerCopy(key_value.key); - if (key == "value") { - value = key_value.value; - continue; - } - if (key == "unit") { - unit = key_value.value; - continue; - } - if (!value.has_value() && hasContent(key_value.value)) { - value = key_value.value; - unit = key_value.key; - } + std::lock_guard lock(state->mutex); + if (!state->active) { + return; } - - const auto normalized_status = mapLevel(status.level); - std::optional alert_message; - if (hasContent(status.message) && normalized_status != DiagnosticStatus::Ok) { - alert_message = status.message; - } - - return { - status.name, - normalized_status, - timestamp, - value, - unit, - alert_message, - }; + state->latest_messages = std::move(normalized_messages); + state->last_received_time = received_at; + state->has_received = true; } } // namespace waybionic_rviz_plugins diff --git a/waybionic_rviz_plugins/test/test_ros_diagnostics_source.cpp b/waybionic_rviz_plugins/test/test_ros_diagnostics_source.cpp new file mode 100644 index 0000000..63448e0 --- /dev/null +++ b/waybionic_rviz_plugins/test/test_ros_diagnostics_source.cpp @@ -0,0 +1,313 @@ +// Regression tests for live/mock diagnostics source handoff. +// +// Before the handoff fix, replacing a live source destroyed RosDiagnosticsSource +// while an executor thread could still be inside its subscription callback. The +// churn tests below reproduce that pattern deterministically: they create, stop +// and destroy live sources in a tight loop while diagnostics traffic is flowing +// on a multi-threaded executor. + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "waybionic_rviz_plugins/diagnostics_contract.hpp" +#include "waybionic_rviz_plugins/ros_diagnostics_source.hpp" + +namespace waybionic_rviz_plugins +{ +namespace +{ + +using namespace std::chrono_literals; + +constexpr const char * kTopic = "/diagnostics_source_test"; + +diagnostic_msgs::msg::DiagnosticArray makeArray(const unsigned char level, const std::string & value) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = "board.temperature"; + status.level = level; + status.message = "temperature reading"; + + diagnostic_msgs::msg::KeyValue value_entry; + value_entry.key = "value"; + value_entry.value = value; + status.values.push_back(value_entry); + + diagnostic_msgs::msg::KeyValue unit_entry; + unit_entry.key = "unit"; + unit_entry.value = "C"; + status.values.push_back(unit_entry); + + diagnostic_msgs::msg::DiagnosticArray array; + array.status.push_back(status); + return array; +} + +template +bool waitFor(Predicate predicate, const std::chrono::milliseconds timeout) +{ + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) { + if (predicate()) { + return true; + } + std::this_thread::sleep_for(5ms); + } + return predicate(); +} + +/// Spins a subscriber node on a multi-threaded executor while a separate thread +/// floods the diagnostics topic, so callbacks genuinely overlap source teardown. +class DiagnosticsTrafficFixture : public ::testing::Test +{ +protected: + void SetUp() override + { + subscriber_node_ = std::make_shared("diagnostics_source_test_subscriber"); + publisher_node_ = std::make_shared("diagnostics_source_test_publisher"); + publisher_ = publisher_node_->create_publisher( + kTopic, rclcpp::QoS(10)); + + executor_ = std::make_shared( + rclcpp::ExecutorOptions(), 4); + executor_->add_node(subscriber_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + stopTraffic(); + executor_->cancel(); + if (executor_thread_.joinable()) { + executor_thread_.join(); + } + executor_->remove_node(subscriber_node_); + publisher_.reset(); + publisher_node_.reset(); + subscriber_node_.reset(); + } + + void startTraffic(const std::chrono::microseconds period = 200us) + { + traffic_running_ = true; + traffic_thread_ = std::thread([this, period]() { + unsigned int counter = 0; + while (traffic_running_) { + const auto level = (counter % 5 == 0) + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_msgs::msg::DiagnosticStatus::OK; + publisher_->publish(makeArray(level, std::to_string(counter))); + ++counter; + published_ = counter; + std::this_thread::sleep_for(period); + } + }); + } + + void stopTraffic() + { + traffic_running_ = false; + if (traffic_thread_.joinable()) { + traffic_thread_.join(); + } + } + + std::unique_ptr makeSource() + { + return std::make_unique(subscriber_node_, kTopic); + } + + rclcpp::Time now() const + { + rclcpp::Clock clock(RCL_SYSTEM_TIME); + return clock.now(); + } + + rclcpp::Node::SharedPtr subscriber_node_; + rclcpp::Node::SharedPtr publisher_node_; + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + std::thread executor_thread_; + std::thread traffic_thread_; + std::atomic traffic_running_{false}; + std::atomic published_{0}; +}; + +TEST_F(DiagnosticsTrafficFixture, ReportsWaitingBeforeAnyMessageArrives) +{ + const auto source = makeSource(); + + const auto messages = source->messages(now()); + ASSERT_EQ(messages.size(), 1u); + EXPECT_EQ(messages.front().signal_name, "diagnostics.topic"); + EXPECT_EQ(messages.front().status, DiagnosticStatus::Stale); + EXPECT_NE(source->connectionStatus(now()).find("Waiting for"), std::string::npos); +} + +TEST_F(DiagnosticsTrafficFixture, NormalizesReceivedDiagnostics) +{ + const auto source = makeSource(); + startTraffic(2ms); + + ASSERT_TRUE(waitFor([&]() { + const auto messages = source->messages(now()); + return messages.size() == 1u && messages.front().signal_name == "board.temperature"; + }, 10s)); + + const auto messages = source->messages(now()); + ASSERT_EQ(messages.size(), 1u); + EXPECT_EQ(messages.front().signal_name, "board.temperature"); + ASSERT_TRUE(messages.front().unit.has_value()); + EXPECT_EQ(*messages.front().unit, "C"); + EXPECT_NE(source->connectionStatus(now()).find("Connected to"), std::string::npos); + + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, StopFreezesStateAndIgnoresLaterMessages) +{ + auto source = makeSource(); + startTraffic(2ms); + + ASSERT_TRUE(waitFor([&]() { + return source->messages(now()).front().signal_name == "board.temperature"; + }, 10s)); + + source->stop(); + EXPECT_TRUE(source->isStopped()); + + const auto frozen = source->messages(now()); + ASSERT_EQ(frozen.size(), 1u); + const auto frozen_value = frozen.front().value; + + // Let a substantial amount of further traffic flow past the retired source. + const auto published_at_stop = published_.load(); + ASSERT_TRUE(waitFor([&]() { return published_.load() > published_at_stop + 50; }, 10s)); + + const auto after = source->messages(now()); + ASSERT_EQ(after.size(), 1u); + EXPECT_EQ(after.front().value, frozen_value) << "retired source must not keep ingesting messages"; + + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, StopIsIdempotent) +{ + auto source = makeSource(); + startTraffic(2ms); + + ASSERT_TRUE(waitFor([&]() { + return source->messages(now()).front().signal_name == "board.temperature"; + }, 10s)); + + source->stop(); + source->stop(); + source->stop(); + EXPECT_TRUE(source->isStopped()); + EXPECT_NO_THROW(source->messages(now())); + EXPECT_NO_THROW(source->connectionStatus(now())); + + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, RepeatedLiveMockChurnUnderTrafficIsSafe) +{ + startTraffic(200us); + + // Mirrors a user toggling "Use Mock Diagnostics" repeatedly while the cycle + // publisher is running. Each iteration replaces the live source exactly the + // way DiagnosticsPanel::configureSource does. + constexpr int kIterations = 300; + for (int iteration = 0; iteration < kIterations; ++iteration) { + auto live_source = makeSource(); + + // Read like the refresh timer does while callbacks are arriving. + (void)live_source->messages(now()); + (void)live_source->connectionStatus(now()); + + live_source->stop(); + live_source.reset(); + } + + EXPECT_GT(published_.load(), 0u); + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, DestructionWithoutExplicitStopIsSafe) +{ + startTraffic(200us); + + constexpr int kIterations = 300; + for (int iteration = 0; iteration < kIterations; ++iteration) { + auto live_source = makeSource(); + (void)live_source->messages(now()); + // No stop() call: the destructor must retire the subscription on its own. + } + + EXPECT_GT(published_.load(), 0u); + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, ConcurrentReadsDuringTeardownAreSafe) +{ + startTraffic(200us); + + constexpr int kIterations = 150; + for (int iteration = 0; iteration < kIterations; ++iteration) { + auto live_source = makeSource(); + + std::atomic reading{true}; + std::thread reader([&]() { + while (reading) { + (void)live_source->messages(now()); + (void)live_source->connectionStatus(now()); + } + }); + + std::this_thread::sleep_for(1ms); + live_source->stop(); + reading = false; + reader.join(); + + live_source.reset(); + } + + stopTraffic(); +} + +TEST_F(DiagnosticsTrafficFixture, ChurnLeavesNoLingeringSubscription) +{ + constexpr int kIterations = 25; + for (int iteration = 0; iteration < kIterations; ++iteration) { + auto live_source = makeSource(); + live_source->stop(); + } + + // A leaked subscription would keep the publisher's subscriber count above zero. + EXPECT_TRUE(waitFor([&]() { + return publisher_node_->count_subscribers(kTopic) == 0u; + }, 15s)) << "expected every retired source to drop its subscription"; +} + +} // namespace +} // namespace waybionic_rviz_plugins + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + const int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}