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; +}