From 14f383586f6df1f30edde7fc044833a87ebed01c Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 21 Mar 2026 21:47:31 +0100 Subject: [PATCH 1/4] feat: add beacon mode switch to sensor diagnostics demo Add BEACON_MODE environment variable (none/topic/param) that controls entity enrichment via gateway beacon plugins. Sensor nodes conditionally publish MedkitDiscoveryHint messages (topic mode) or declare ros2_medkit.discovery.* parameters (param mode) with sensor-specific metadata (entity_id, display_name, component, function membership, sensor type, data topic, frame ID). Shared BeaconHelper class avoids code duplication across 4 sensor nodes. Launch file resolves the appropriate beacon plugin based on mode. Docker-compose passes BEACON_MODE through to the container. Closes #41 --- demos/sensor_diagnostics/CMakeLists.txt | 4 + demos/sensor_diagnostics/README.md | 53 ++++++++ demos/sensor_diagnostics/docker-compose.yml | 2 + .../sensor_diagnostics/launch/demo.launch.py | 81 +++++++++-- .../sensor_diagnostics/src/beacon_helper.hpp | 127 ++++++++++++++++++ .../src/camera_sim_node.cpp | 12 ++ demos/sensor_diagnostics/src/gps_sim_node.cpp | 12 ++ demos/sensor_diagnostics/src/imu_sim_node.cpp | 12 ++ .../sensor_diagnostics/src/lidar_sim_node.cpp | 12 ++ 9 files changed, 301 insertions(+), 14 deletions(-) create mode 100644 demos/sensor_diagnostics/src/beacon_helper.hpp diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt index 09144bc..648ac07 100644 --- a/demos/sensor_diagnostics/CMakeLists.txt +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -22,6 +22,7 @@ ament_target_dependencies(lidar_sim_node sensor_msgs diagnostic_msgs rcl_interfaces + ros2_medkit_msgs ) # IMU simulator node @@ -30,6 +31,7 @@ ament_target_dependencies(imu_sim_node rclcpp sensor_msgs diagnostic_msgs + ros2_medkit_msgs ) # GPS simulator node @@ -38,6 +40,7 @@ ament_target_dependencies(gps_sim_node rclcpp sensor_msgs diagnostic_msgs + ros2_medkit_msgs ) # Camera simulator node @@ -47,6 +50,7 @@ ament_target_dependencies(camera_sim_node sensor_msgs diagnostic_msgs rcl_interfaces + ros2_medkit_msgs ) # Anomaly detector node diff --git a/demos/sensor_diagnostics/README.md b/demos/sensor_diagnostics/README.md index 4c05628..976ee02 100644 --- a/demos/sensor_diagnostics/README.md +++ b/demos/sensor_diagnostics/README.md @@ -11,6 +11,7 @@ This demo showcases ros2_medkit's data monitoring, configuration management, and - **Focus on diagnostics** - Pure ros2_medkit features without robot complexity - **Configurable faults** - Runtime fault injection via REST API - **Dual fault reporting** - Demonstrates both legacy (diagnostics) and modern (direct) paths +- **Beacon discovery** - Optional push (topic) or pull (parameter) entity enrichment ## Quick Start @@ -255,6 +256,58 @@ curl http://localhost:8080/api/v1/faults | jq | `brightness` | int | 128 | Base brightness (0-255) | | `inject_black_frames` | bool | false | Randomly inject black frames | +## Beacon Mode (Entity Enrichment) + +The gateway's beacon plugins let sensor nodes publish extra metadata (display names, process info, topology hints) that enriches the SOVD entity model at runtime - without modifying the manifest. + +Three modes are available, controlled by the `BEACON_MODE` environment variable: + +| Mode | Plugin | Mechanism | Description | +|------|--------|-----------|-------------| +| `none` | - | - | Default. No beacon plugins. Entities come from manifest + runtime discovery only. | +| `topic` | topic_beacon | Push (ROS 2 topic) | Sensor nodes publish `MedkitDiscoveryHint` messages on `/ros2_medkit/discovery` every 5s. Gateway subscribes and enriches entities. | +| `param` | parameter_beacon | Pull (ROS 2 parameters) | Sensor nodes declare `ros2_medkit.discovery.*` parameters. Gateway polls them every 5s. | + +### Usage + +```bash +# Docker - set BEACON_MODE before starting +BEACON_MODE=topic docker compose up -d +BEACON_MODE=param docker compose up -d +docker compose up -d # default: none + +# Local (non-Docker) +BEACON_MODE=topic ros2 launch sensor_diagnostics_demo demo.launch.py +``` + +### Viewing Beacon Data + +When a beacon mode is active, each sensor entity gets enriched with extra metadata visible through the API: + +```bash +# Topic beacon metadata +curl http://localhost:8080/api/v1/apps/lidar-sim/x-medkit-topic-beacon | jq + +# Parameter beacon metadata +curl http://localhost:8080/api/v1/apps/lidar-sim/x-medkit-param-beacon | jq +``` + +The beacon data includes: +- **entity_id** - Manifest app ID (e.g., `lidar-sim`) +- **display_name** - Human-friendly name (e.g., `LiDAR Simulator`) +- **component_id** - Parent component (e.g., `lidar-unit`) +- **function_ids** - Function membership (e.g., `sensor-monitoring`) +- **process_id** / **hostname** - Process-level diagnostics +- **metadata** - Sensor-specific key-value pairs (sensor_type, data_topic, frame_id) + +### How It Works + +**Topic beacon** (push): Each sensor node creates a publisher on `/ros2_medkit/discovery` and publishes a `MedkitDiscoveryHint` message every 5 seconds. The gateway's `topic_beacon` plugin subscribes to this topic and merges the hints into the entity model. Hints have a TTL (default 10s) - if a node stops publishing, the data goes stale. + +**Parameter beacon** (pull): Each sensor node declares ROS 2 parameters under the `ros2_medkit.discovery.*` prefix. The gateway's `parameter_beacon` plugin polls all nodes for these parameters every 5 seconds. No explicit publishing is needed - the gateway reads the parameters via the ROS 2 parameter service. + +Both mechanisms enrich the same entities defined in the manifest. They do not create new entities (the `allow_new_entities` option is disabled). Only one beacon mode should be active at a time - they serve the same purpose via different transport mechanisms. + ## Use Cases 1. **CI/CD Testing** - Validate ros2_medkit without heavy simulation diff --git a/demos/sensor_diagnostics/docker-compose.yml b/demos/sensor_diagnostics/docker-compose.yml index 5048fc0..0a2b452 100644 --- a/demos/sensor_diagnostics/docker-compose.yml +++ b/demos/sensor_diagnostics/docker-compose.yml @@ -7,6 +7,7 @@ services: container_name: sensor_diagnostics_demo environment: - ROS_DOMAIN_ID=40 + - BEACON_MODE=${BEACON_MODE:-none} ports: - "8080:8080" stdin_open: true @@ -37,6 +38,7 @@ services: container_name: sensor_diagnostics_demo_ci environment: - ROS_DOMAIN_ID=40 + - BEACON_MODE=${BEACON_MODE:-none} ports: - "8080:8080" command: > diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index f34815b..9bfd078 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -3,14 +3,19 @@ Lightweight demo without Gazebo - pure sensor simulation with fault injection. Demonstrates two fault reporting paths: -1. Legacy path: Sensors → /diagnostics topic → diagnostic_bridge → fault_manager +1. Legacy path: Sensors -> /diagnostics topic -> diagnostic_bridge -> fault_manager - Used by: LiDAR, Camera - Standard ROS 2 diagnostics pattern -2. Modern path: Sensors → anomaly_detector → ReportFault service → fault_manager +2. Modern path: Sensors -> anomaly_detector -> ReportFault service -> fault_manager - Used by: IMU, GPS - Direct ros2_medkit fault reporting +Beacon modes (set via BEACON_MODE env var): + none - No beacon plugins (default) + topic - Topic beacon: sensor nodes push MedkitDiscoveryHint messages + param - Parameter beacon: gateway polls sensor node parameters + Namespace structure: /sensors - Simulated sensor nodes (lidar, imu, gps, camera) /processing - Anomaly detector @@ -50,6 +55,9 @@ def generate_launch_description(): sensor_params_file = os.path.join(pkg_dir, "config", "sensor_params.yaml") manifest_file = os.path.join(pkg_dir, "config", "sensor_manifest.yaml") + # Beacon mode from environment (controls both plugin loading and node behavior) + beacon_mode = os.environ.get('BEACON_MODE', 'none') + # Resolve plugin paths graph_provider_path = _resolve_plugin_path( 'ros2_medkit_graph_provider', 'ros2_medkit_graph_provider_plugin') @@ -65,8 +73,32 @@ def generate_launch_description(): if procfs_plugin_path: active_plugins.append('procfs_introspection') plugin_overrides['plugins.procfs_introspection.path'] = procfs_plugin_path + + # Beacon plugin (mutually exclusive - only one beacon type at a time) + if beacon_mode == 'topic': + topic_beacon_path = _resolve_plugin_path( + 'ros2_medkit_topic_beacon', 'topic_beacon_plugin') + if topic_beacon_path: + active_plugins.append('topic_beacon') + plugin_overrides['plugins.topic_beacon.path'] = topic_beacon_path + plugin_overrides['plugins.topic_beacon.topic'] = \ + '/ros2_medkit/discovery' + plugin_overrides['plugins.topic_beacon.beacon_ttl_sec'] = 10.0 + elif beacon_mode == 'param': + param_beacon_path = _resolve_plugin_path( + 'ros2_medkit_param_beacon', 'param_beacon_plugin') + if param_beacon_path: + active_plugins.append('parameter_beacon') + plugin_overrides['plugins.parameter_beacon.path'] = \ + param_beacon_path + plugin_overrides['plugins.parameter_beacon.poll_interval_sec'] = \ + 5.0 + plugin_overrides['plugins'] = active_plugins + # Sensor node beacon parameter (passed to all sensor nodes) + beacon_params = {"beacon_mode": beacon_mode} + # Launch arguments use_sim_time = LaunchConfiguration("use_sim_time", default="false") @@ -76,7 +108,8 @@ def generate_launch_description(): DeclareLaunchArgument( "use_sim_time", default_value="false", - description="Use simulation time (set to true if using with Gazebo)", + description="Use simulation time (set to true if using " + "with Gazebo)", ), # ===== Sensor Nodes (under /sensors namespace) ===== # Legacy path sensors: publish DiagnosticArray to /diagnostics @@ -86,7 +119,11 @@ def generate_launch_description(): name="lidar_sim", namespace="sensors", output="screen", - parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + parameters=[ + sensor_params_file, + {"use_sim_time": use_sim_time}, + beacon_params, + ], ), Node( package="sensor_diagnostics_demo", @@ -94,16 +131,24 @@ def generate_launch_description(): name="camera_sim", namespace="sensors", output="screen", - parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + parameters=[ + sensor_params_file, + {"use_sim_time": use_sim_time}, + beacon_params, + ], ), - # Modern path sensors: monitored by anomaly_detector → ReportFault + # Modern path sensors: monitored by anomaly_detector -> ReportFault Node( package="sensor_diagnostics_demo", executable="imu_sim_node", name="imu_sim", namespace="sensors", output="screen", - parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + parameters=[ + sensor_params_file, + {"use_sim_time": use_sim_time}, + beacon_params, + ], ), Node( package="sensor_diagnostics_demo", @@ -111,20 +156,27 @@ def generate_launch_description(): name="gps_sim", namespace="sensors", output="screen", - parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + parameters=[ + sensor_params_file, + {"use_sim_time": use_sim_time}, + beacon_params, + ], ), # ===== Processing Nodes (under /processing namespace) ===== - # Modern path: anomaly_detector monitors IMU/GPS and calls ReportFault + # Modern path: anomaly_detector monitors IMU/GPS, calls ReportFault Node( package="sensor_diagnostics_demo", executable="anomaly_detector_node", name="anomaly_detector", namespace="processing", output="screen", - parameters=[sensor_params_file, {"use_sim_time": use_sim_time}], + parameters=[ + sensor_params_file, + {"use_sim_time": use_sim_time}, + ], ), # ===== Diagnostic Bridge (Legacy path) ===== - # Bridges /diagnostics topic (DiagnosticArray) → fault_manager + # Bridges /diagnostics topic (DiagnosticArray) -> fault_manager # Handles faults from: LiDAR, Camera Node( package="ros2_medkit_diagnostic_bridge", @@ -156,13 +208,14 @@ def generate_launch_description(): ), # ===== Fault Manager (at root namespace) ===== # Services at /fault_manager/* (e.g., /fault_manager/report_fault) - # Both paths report here: diagnostic_bridge (legacy) and anomaly_detector (modern) - # Also handles snapshot and rosbag capture when faults are confirmed + # Both paths report here: diagnostic_bridge (legacy) and + # anomaly_detector (modern) + # Also handles snapshot and rosbag capture on fault confirmation Node( package="ros2_medkit_fault_manager", executable="fault_manager_node", name="fault_manager", - namespace="", # Root namespace so services are at /fault_manager/* + namespace="", # Root namespace: services at /fault_manager/* output="screen", parameters=[ medkit_params_file, diff --git a/demos/sensor_diagnostics/src/beacon_helper.hpp b/demos/sensor_diagnostics/src/beacon_helper.hpp new file mode 100644 index 0000000..c177169 --- /dev/null +++ b/demos/sensor_diagnostics/src/beacon_helper.hpp @@ -0,0 +1,127 @@ +// Copyright 2026 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +/// @file beacon_helper.hpp +/// @brief Shared helper for beacon mode support in sensor demo nodes +/// +/// Encapsulates topic beacon (push) and parameter beacon (pull) logic. +/// Each sensor node creates a BeaconHelper with its entity config. +/// The beacon_mode parameter ("none", "topic", "param") controls behavior. + +#pragma once + +#include +#include +#include +#include + +#include + +#include "diagnostic_msgs/msg/key_value.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ros2_medkit_msgs/msg/medkit_discovery_hint.hpp" + +namespace sensor_diagnostics +{ + +class BeaconHelper +{ +public: + struct Config + { + std::string entity_id; + std::string display_name; + std::string component_id; + std::vector function_ids; + std::unordered_map metadata; + }; + + BeaconHelper(rclcpp::Node * node, const Config & config) + : node_(node), config_(config) + { + node_->declare_parameter("beacon_mode", "none"); + mode_ = node_->get_parameter("beacon_mode").as_string(); + + if (mode_ == "topic") { + init_topic_beacon(); + } else if (mode_ == "param") { + init_param_beacon(); + } + } + +private: + void init_topic_beacon() + { + beacon_pub_ = node_->create_publisher( + "/ros2_medkit/discovery", 10); + + beacon_timer_ = node_->create_wall_timer( + std::chrono::seconds(5), + [this]() { publish_beacon(); }); + + // Publish immediately on startup + publish_beacon(); + + RCLCPP_INFO( + node_->get_logger(), "Topic beacon enabled (entity: %s)", + config_.entity_id.c_str()); + } + + void init_param_beacon() + { + node_->declare_parameter("ros2_medkit.discovery.entity_id", config_.entity_id); + node_->declare_parameter("ros2_medkit.discovery.display_name", config_.display_name); + node_->declare_parameter("ros2_medkit.discovery.component_id", config_.component_id); + node_->declare_parameter("ros2_medkit.discovery.function_ids", config_.function_ids); + node_->declare_parameter( + "ros2_medkit.discovery.process_id", static_cast(getpid())); + node_->declare_parameter("ros2_medkit.discovery.process_name", node_->get_name()); + + char hostname_buf[256]; + if (gethostname(hostname_buf, sizeof(hostname_buf)) == 0) { + node_->declare_parameter("ros2_medkit.discovery.hostname", std::string(hostname_buf)); + } + + for (const auto & [key, value] : config_.metadata) { + node_->declare_parameter("ros2_medkit.discovery.metadata." + key, value); + } + + RCLCPP_INFO( + node_->get_logger(), "Parameter beacon enabled (entity: %s)", + config_.entity_id.c_str()); + } + + void publish_beacon() + { + auto msg = ros2_medkit_msgs::msg::MedkitDiscoveryHint(); + msg.entity_id = config_.entity_id; + msg.display_name = config_.display_name; + msg.component_id = config_.component_id; + msg.function_ids = config_.function_ids; + msg.process_id = static_cast(getpid()); + msg.process_name = node_->get_name(); + msg.stamp = node_->now(); + + char hostname_buf[256]; + if (gethostname(hostname_buf, sizeof(hostname_buf)) == 0) { + msg.hostname = hostname_buf; + } + + for (const auto & [key, value] : config_.metadata) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = key; + kv.value = value; + msg.metadata.push_back(kv); + } + + beacon_pub_->publish(msg); + } + + rclcpp::Node * node_; + Config config_; + std::string mode_; + rclcpp::Publisher::SharedPtr beacon_pub_; + rclcpp::TimerBase::SharedPtr beacon_timer_; +}; + +} // namespace sensor_diagnostics diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index 6a5e228..bfa5ca3 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -24,6 +24,8 @@ #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" +#include "beacon_helper.hpp" + namespace sensor_diagnostics { @@ -71,6 +73,13 @@ class CameraSimNode : public rclcpp::Node param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&CameraSimNode::on_parameter_change, this, std::placeholders::_1)); + // Initialize beacon (reads beacon_mode parameter: none/topic/param) + beacon_ = std::make_unique(this, BeaconHelper::Config{ + "camera-sim", "Camera Simulator", "camera-unit", + {"sensor-monitoring"}, + {{"sensor_type", "camera"}, {"data_topic", "image_raw"}, {"frame_id", "camera_link"}}, + }); + RCLCPP_INFO( this->get_logger(), "Camera simulator started at %.1f Hz (%dx%d)", rate, width_, height_); @@ -258,6 +267,9 @@ class CameraSimNode : public rclcpp::Node std::mt19937 rng_; std::uniform_real_distribution uniform_dist_; + // Beacon + std::unique_ptr beacon_; + // Parameters int width_; int height_; diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp index 0583e9f..c96663b 100644 --- a/demos/sensor_diagnostics/src/gps_sim_node.cpp +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -21,6 +21,8 @@ #include "sensor_msgs/msg/nav_sat_fix.hpp" #include "sensor_msgs/msg/nav_sat_status.hpp" +#include "beacon_helper.hpp" + namespace sensor_diagnostics { @@ -71,6 +73,13 @@ class GpsSimNode : public rclcpp::Node param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&GpsSimNode::on_parameter_change, this, std::placeholders::_1)); + // Initialize beacon (reads beacon_mode parameter: none/topic/param) + beacon_ = std::make_unique(this, BeaconHelper::Config{ + "gps-sim", "GPS Simulator", "gps-unit", + {"sensor-monitoring"}, + {{"sensor_type", "gps"}, {"data_topic", "fix"}, {"frame_id", "gps_link"}}, + }); + RCLCPP_INFO(this->get_logger(), "GPS simulator started at %.1f Hz", rate); } @@ -242,6 +251,9 @@ class GpsSimNode : public rclcpp::Node std::normal_distribution normal_dist_; std::uniform_real_distribution uniform_dist_; + // Beacon + std::unique_ptr beacon_; + // Parameters double base_latitude_; double base_longitude_; diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp index 65f122d..16ccba9 100644 --- a/demos/sensor_diagnostics/src/imu_sim_node.cpp +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -20,6 +20,8 @@ #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "beacon_helper.hpp" + namespace sensor_diagnostics { @@ -67,6 +69,13 @@ class ImuSimNode : public rclcpp::Node param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&ImuSimNode::on_parameter_change, this, std::placeholders::_1)); + // Initialize beacon (reads beacon_mode parameter: none/topic/param) + beacon_ = std::make_unique(this, BeaconHelper::Config{ + "imu-sim", "IMU Simulator", "imu-unit", + {"sensor-monitoring"}, + {{"sensor_type", "imu"}, {"data_topic", "imu"}, {"frame_id", "imu_link"}}, + }); + RCLCPP_INFO(this->get_logger(), "IMU simulator started at %.1f Hz", rate); } @@ -220,6 +229,9 @@ class ImuSimNode : public rclcpp::Node std::normal_distribution normal_dist_; std::uniform_real_distribution uniform_dist_; + // Beacon + std::unique_ptr beacon_; + // Parameters double accel_noise_stddev_; double gyro_noise_stddev_; diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index 068ebaf..ddd4762 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -25,6 +25,8 @@ #include "rcl_interfaces/msg/set_parameters_result.hpp" #include "sensor_msgs/msg/laser_scan.hpp" +#include "beacon_helper.hpp" + namespace sensor_diagnostics { @@ -80,6 +82,13 @@ class LidarSimNode : public rclcpp::Node param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&LidarSimNode::on_parameter_change, this, std::placeholders::_1)); + // Initialize beacon (reads beacon_mode parameter: none/topic/param) + beacon_ = std::make_unique(this, BeaconHelper::Config{ + "lidar-sim", "LiDAR Simulator", "lidar-unit", + {"sensor-monitoring"}, + {{"sensor_type", "lidar"}, {"data_topic", "scan"}, {"frame_id", "lidar_link"}}, + }); + RCLCPP_INFO(this->get_logger(), "LiDAR simulator started at %.1f Hz", rate); } @@ -309,6 +318,9 @@ class LidarSimNode : public rclcpp::Node bool inject_nan_; double drift_rate_; + // Beacon + std::unique_ptr beacon_; + // Statistics rclcpp::Time start_time_; uint64_t msg_count_{0}; From b50797e1cbda012b50fc2fcc0ec4a99163727f3a Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 21 Mar 2026 22:02:28 +0100 Subject: [PATCH 2/4] fix: validate BEACON_MODE, cache process info, handle missing plugins Address self-review findings: - Warn and fall back to 'none' for invalid BEACON_MODE values - Warn and fall back to 'none' when beacon plugin .so is not found - Cache pid and hostname in constructor instead of calling syscalls on every publish - Add explicit include - Null-terminate hostname buffer to prevent truncation UB --- .../sensor_diagnostics/launch/demo.launch.py | 26 ++++++++++++++++ .../sensor_diagnostics/src/beacon_helper.hpp | 31 +++++++++++++------ 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index 9bfd078..7995341 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -57,6 +57,16 @@ def generate_launch_description(): # Beacon mode from environment (controls both plugin loading and node behavior) beacon_mode = os.environ.get('BEACON_MODE', 'none') + valid_beacon_modes = ('none', 'topic', 'param') + if beacon_mode not in valid_beacon_modes: + import sys + print( + f"WARNING: Invalid BEACON_MODE='{beacon_mode}'. " + f"Valid values: {', '.join(valid_beacon_modes)}. " + "Falling back to 'none'.", + file=sys.stderr, + ) + beacon_mode = 'none' # Resolve plugin paths graph_provider_path = _resolve_plugin_path( @@ -84,6 +94,14 @@ def generate_launch_description(): plugin_overrides['plugins.topic_beacon.topic'] = \ '/ros2_medkit/discovery' plugin_overrides['plugins.topic_beacon.beacon_ttl_sec'] = 10.0 + else: + import sys + print( + "WARNING: BEACON_MODE=topic but topic_beacon plugin not " + "found. Falling back to none.", + file=sys.stderr, + ) + beacon_mode = 'none' elif beacon_mode == 'param': param_beacon_path = _resolve_plugin_path( 'ros2_medkit_param_beacon', 'param_beacon_plugin') @@ -93,6 +111,14 @@ def generate_launch_description(): param_beacon_path plugin_overrides['plugins.parameter_beacon.poll_interval_sec'] = \ 5.0 + else: + import sys + print( + "WARNING: BEACON_MODE=param but param_beacon plugin not " + "found. Falling back to none.", + file=sys.stderr, + ) + beacon_mode = 'none' plugin_overrides['plugins'] = active_plugins diff --git a/demos/sensor_diagnostics/src/beacon_helper.hpp b/demos/sensor_diagnostics/src/beacon_helper.hpp index c177169..61a98c5 100644 --- a/demos/sensor_diagnostics/src/beacon_helper.hpp +++ b/demos/sensor_diagnostics/src/beacon_helper.hpp @@ -10,6 +10,7 @@ #pragma once +#include #include #include #include @@ -42,10 +43,23 @@ class BeaconHelper node_->declare_parameter("beacon_mode", "none"); mode_ = node_->get_parameter("beacon_mode").as_string(); + // Cache process info (constant during lifetime) + pid_ = static_cast(getpid()); + char hostname_buf[256]; + hostname_buf[sizeof(hostname_buf) - 1] = '\0'; + if (gethostname(hostname_buf, sizeof(hostname_buf) - 1) == 0) { + hostname_ = hostname_buf; + } + if (mode_ == "topic") { init_topic_beacon(); } else if (mode_ == "param") { init_param_beacon(); + } else if (mode_ != "none") { + RCLCPP_WARN( + node_->get_logger(), + "Unknown beacon_mode '%s', expected none/topic/param. Beacon disabled.", + mode_.c_str()); } } @@ -74,12 +88,11 @@ class BeaconHelper node_->declare_parameter("ros2_medkit.discovery.component_id", config_.component_id); node_->declare_parameter("ros2_medkit.discovery.function_ids", config_.function_ids); node_->declare_parameter( - "ros2_medkit.discovery.process_id", static_cast(getpid())); + "ros2_medkit.discovery.process_id", static_cast(pid_)); node_->declare_parameter("ros2_medkit.discovery.process_name", node_->get_name()); - char hostname_buf[256]; - if (gethostname(hostname_buf, sizeof(hostname_buf)) == 0) { - node_->declare_parameter("ros2_medkit.discovery.hostname", std::string(hostname_buf)); + if (!hostname_.empty()) { + node_->declare_parameter("ros2_medkit.discovery.hostname", hostname_); } for (const auto & [key, value] : config_.metadata) { @@ -98,15 +111,11 @@ class BeaconHelper msg.display_name = config_.display_name; msg.component_id = config_.component_id; msg.function_ids = config_.function_ids; - msg.process_id = static_cast(getpid()); + msg.process_id = pid_; msg.process_name = node_->get_name(); + msg.hostname = hostname_; msg.stamp = node_->now(); - char hostname_buf[256]; - if (gethostname(hostname_buf, sizeof(hostname_buf)) == 0) { - msg.hostname = hostname_buf; - } - for (const auto & [key, value] : config_.metadata) { diagnostic_msgs::msg::KeyValue kv; kv.key = key; @@ -120,6 +129,8 @@ class BeaconHelper rclcpp::Node * node_; Config config_; std::string mode_; + uint32_t pid_{0}; + std::string hostname_; rclcpp::Publisher::SharedPtr beacon_pub_; rclcpp::TimerBase::SharedPtr beacon_timer_; }; From 16adec2c707f8106470c86af61a879dbe1c4ee48 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sat, 21 Mar 2026 22:32:17 +0100 Subject: [PATCH 3/4] fix: normalize BEACON_MODE with strip().lower() Accept case-insensitive input (e.g., BEACON_MODE=Topic). --- demos/sensor_diagnostics/launch/demo.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index 7995341..65d3c62 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -56,7 +56,7 @@ def generate_launch_description(): manifest_file = os.path.join(pkg_dir, "config", "sensor_manifest.yaml") # Beacon mode from environment (controls both plugin loading and node behavior) - beacon_mode = os.environ.get('BEACON_MODE', 'none') + beacon_mode = os.environ.get('BEACON_MODE', 'none').strip().lower() valid_beacon_modes = ('none', 'topic', 'param') if beacon_mode not in valid_beacon_modes: import sys From 45940200138f2af24f88b4372a95599692d59138 Mon Sep 17 00:00:00 2001 From: Bartosz Burda Date: Sun, 22 Mar 2026 12:49:18 +0100 Subject: [PATCH 4/4] fix: address review feedback on beacon mode PR - Split beacon_helper.hpp into .hpp/.cpp with static library - Use std::map for stable metadata key ordering - Use int64_t for process_id parameter to avoid truncation - Make entity_id a node parameter with default (reduces manifest coupling) - Move import sys to top-level in launch file - Increase beacon TTL from 10s to 15s (3x publish interval) - Hardcode BEACON_MODE=none in CI service for deterministic tests --- demos/sensor_diagnostics/CMakeLists.txt | 19 +++- demos/sensor_diagnostics/docker-compose.yml | 2 +- .../sensor_diagnostics/launch/demo.launch.py | 6 +- .../sensor_diagnostics/src/beacon_helper.cpp | 101 ++++++++++++++++++ .../sensor_diagnostics/src/beacon_helper.hpp | 97 ++--------------- .../src/camera_sim_node.cpp | 3 +- demos/sensor_diagnostics/src/gps_sim_node.cpp | 3 +- demos/sensor_diagnostics/src/imu_sim_node.cpp | 3 +- .../sensor_diagnostics/src/lidar_sim_node.cpp | 3 +- 9 files changed, 133 insertions(+), 104 deletions(-) create mode 100644 demos/sensor_diagnostics/src/beacon_helper.cpp diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt index 648ac07..c124333 100644 --- a/demos/sensor_diagnostics/CMakeLists.txt +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -15,42 +15,53 @@ find_package(diagnostic_msgs REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(ros2_medkit_msgs REQUIRED) +# Beacon helper library (shared across sensor nodes) +add_library(beacon_helper STATIC src/beacon_helper.cpp) +ament_target_dependencies(beacon_helper PUBLIC + rclcpp + diagnostic_msgs + ros2_medkit_msgs +) +target_include_directories(beacon_helper PUBLIC + $ +) + # LiDAR simulator node add_executable(lidar_sim_node src/lidar_sim_node.cpp) +target_link_libraries(lidar_sim_node beacon_helper) ament_target_dependencies(lidar_sim_node rclcpp sensor_msgs diagnostic_msgs rcl_interfaces - ros2_medkit_msgs ) # IMU simulator node add_executable(imu_sim_node src/imu_sim_node.cpp) +target_link_libraries(imu_sim_node beacon_helper) ament_target_dependencies(imu_sim_node rclcpp sensor_msgs diagnostic_msgs - ros2_medkit_msgs ) # GPS simulator node add_executable(gps_sim_node src/gps_sim_node.cpp) +target_link_libraries(gps_sim_node beacon_helper) ament_target_dependencies(gps_sim_node rclcpp sensor_msgs diagnostic_msgs - ros2_medkit_msgs ) # Camera simulator node add_executable(camera_sim_node src/camera_sim_node.cpp) +target_link_libraries(camera_sim_node beacon_helper) ament_target_dependencies(camera_sim_node rclcpp sensor_msgs diagnostic_msgs rcl_interfaces - ros2_medkit_msgs ) # Anomaly detector node diff --git a/demos/sensor_diagnostics/docker-compose.yml b/demos/sensor_diagnostics/docker-compose.yml index 0a2b452..a09019f 100644 --- a/demos/sensor_diagnostics/docker-compose.yml +++ b/demos/sensor_diagnostics/docker-compose.yml @@ -38,7 +38,7 @@ services: container_name: sensor_diagnostics_demo_ci environment: - ROS_DOMAIN_ID=40 - - BEACON_MODE=${BEACON_MODE:-none} + - BEACON_MODE=none ports: - "8080:8080" command: > diff --git a/demos/sensor_diagnostics/launch/demo.launch.py b/demos/sensor_diagnostics/launch/demo.launch.py index 65d3c62..16057e1 100644 --- a/demos/sensor_diagnostics/launch/demo.launch.py +++ b/demos/sensor_diagnostics/launch/demo.launch.py @@ -24,6 +24,7 @@ """ import os +import sys from ament_index_python.packages import get_package_prefix from ament_index_python.packages import get_package_share_directory @@ -59,7 +60,6 @@ def generate_launch_description(): beacon_mode = os.environ.get('BEACON_MODE', 'none').strip().lower() valid_beacon_modes = ('none', 'topic', 'param') if beacon_mode not in valid_beacon_modes: - import sys print( f"WARNING: Invalid BEACON_MODE='{beacon_mode}'. " f"Valid values: {', '.join(valid_beacon_modes)}. " @@ -93,9 +93,8 @@ def generate_launch_description(): plugin_overrides['plugins.topic_beacon.path'] = topic_beacon_path plugin_overrides['plugins.topic_beacon.topic'] = \ '/ros2_medkit/discovery' - plugin_overrides['plugins.topic_beacon.beacon_ttl_sec'] = 10.0 + plugin_overrides['plugins.topic_beacon.beacon_ttl_sec'] = 15.0 else: - import sys print( "WARNING: BEACON_MODE=topic but topic_beacon plugin not " "found. Falling back to none.", @@ -112,7 +111,6 @@ def generate_launch_description(): plugin_overrides['plugins.parameter_beacon.poll_interval_sec'] = \ 5.0 else: - import sys print( "WARNING: BEACON_MODE=param but param_beacon plugin not " "found. Falling back to none.", diff --git a/demos/sensor_diagnostics/src/beacon_helper.cpp b/demos/sensor_diagnostics/src/beacon_helper.cpp new file mode 100644 index 0000000..3b6b606 --- /dev/null +++ b/demos/sensor_diagnostics/src/beacon_helper.cpp @@ -0,0 +1,101 @@ +// Copyright 2026 selfpatch +// SPDX-License-Identifier: Apache-2.0 + +#include "beacon_helper.hpp" + +#include + +#include "diagnostic_msgs/msg/key_value.hpp" + +namespace sensor_diagnostics +{ + +BeaconHelper::BeaconHelper(rclcpp::Node * node, const Config & config) +: node_(node), config_(config) +{ + node_->declare_parameter("beacon_mode", "none"); + mode_ = node_->get_parameter("beacon_mode").as_string(); + + // Cache process info (constant during lifetime) + pid_ = static_cast(getpid()); + char hostname_buf[256]; + hostname_buf[sizeof(hostname_buf) - 1] = '\0'; + if (gethostname(hostname_buf, sizeof(hostname_buf) - 1) == 0) { + hostname_ = hostname_buf; + } + + if (mode_ == "topic") { + init_topic_beacon(); + } else if (mode_ == "param") { + init_param_beacon(); + } else if (mode_ != "none") { + RCLCPP_WARN( + node_->get_logger(), + "Unknown beacon_mode '%s', expected none/topic/param. Beacon disabled.", + mode_.c_str()); + } +} + +void BeaconHelper::init_topic_beacon() +{ + beacon_pub_ = node_->create_publisher( + "/ros2_medkit/discovery", 10); + + beacon_timer_ = node_->create_wall_timer( + std::chrono::seconds(5), + [this]() { publish_beacon(); }); + + // First publish likely dropped due to DDS discovery delay, next arrives in 5s + publish_beacon(); + + RCLCPP_INFO( + node_->get_logger(), "Topic beacon enabled (entity: %s)", + config_.entity_id.c_str()); +} + +void BeaconHelper::init_param_beacon() +{ + node_->declare_parameter("ros2_medkit.discovery.entity_id", config_.entity_id); + node_->declare_parameter("ros2_medkit.discovery.display_name", config_.display_name); + node_->declare_parameter("ros2_medkit.discovery.component_id", config_.component_id); + node_->declare_parameter("ros2_medkit.discovery.function_ids", config_.function_ids); + node_->declare_parameter( + "ros2_medkit.discovery.process_id", static_cast(pid_)); + node_->declare_parameter("ros2_medkit.discovery.process_name", node_->get_name()); + + if (!hostname_.empty()) { + node_->declare_parameter("ros2_medkit.discovery.hostname", hostname_); + } + + for (const auto & [key, value] : config_.metadata) { + node_->declare_parameter("ros2_medkit.discovery.metadata." + key, value); + } + + RCLCPP_INFO( + node_->get_logger(), "Parameter beacon enabled (entity: %s)", + config_.entity_id.c_str()); +} + +void BeaconHelper::publish_beacon() +{ + auto msg = ros2_medkit_msgs::msg::MedkitDiscoveryHint(); + msg.entity_id = config_.entity_id; + msg.display_name = config_.display_name; + msg.component_id = config_.component_id; + msg.function_ids = config_.function_ids; + msg.process_id = pid_; + msg.process_name = node_->get_name(); + msg.hostname = hostname_; + msg.stamp = node_->now(); + + for (const auto & [key, value] : config_.metadata) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = key; + kv.value = value; + msg.metadata.push_back(kv); + } + + beacon_pub_->publish(msg); +} + +} // namespace sensor_diagnostics diff --git a/demos/sensor_diagnostics/src/beacon_helper.hpp b/demos/sensor_diagnostics/src/beacon_helper.hpp index 61a98c5..3e21d63 100644 --- a/demos/sensor_diagnostics/src/beacon_helper.hpp +++ b/demos/sensor_diagnostics/src/beacon_helper.hpp @@ -11,14 +11,11 @@ #pragma once #include +#include #include #include -#include #include -#include - -#include "diagnostic_msgs/msg/key_value.hpp" #include "rclcpp/rclcpp.hpp" #include "ros2_medkit_msgs/msg/medkit_discovery_hint.hpp" @@ -34,97 +31,15 @@ class BeaconHelper std::string display_name; std::string component_id; std::vector function_ids; - std::unordered_map metadata; + std::map metadata; }; - BeaconHelper(rclcpp::Node * node, const Config & config) - : node_(node), config_(config) - { - node_->declare_parameter("beacon_mode", "none"); - mode_ = node_->get_parameter("beacon_mode").as_string(); - - // Cache process info (constant during lifetime) - pid_ = static_cast(getpid()); - char hostname_buf[256]; - hostname_buf[sizeof(hostname_buf) - 1] = '\0'; - if (gethostname(hostname_buf, sizeof(hostname_buf) - 1) == 0) { - hostname_ = hostname_buf; - } - - if (mode_ == "topic") { - init_topic_beacon(); - } else if (mode_ == "param") { - init_param_beacon(); - } else if (mode_ != "none") { - RCLCPP_WARN( - node_->get_logger(), - "Unknown beacon_mode '%s', expected none/topic/param. Beacon disabled.", - mode_.c_str()); - } - } + BeaconHelper(rclcpp::Node * node, const Config & config); private: - void init_topic_beacon() - { - beacon_pub_ = node_->create_publisher( - "/ros2_medkit/discovery", 10); - - beacon_timer_ = node_->create_wall_timer( - std::chrono::seconds(5), - [this]() { publish_beacon(); }); - - // Publish immediately on startup - publish_beacon(); - - RCLCPP_INFO( - node_->get_logger(), "Topic beacon enabled (entity: %s)", - config_.entity_id.c_str()); - } - - void init_param_beacon() - { - node_->declare_parameter("ros2_medkit.discovery.entity_id", config_.entity_id); - node_->declare_parameter("ros2_medkit.discovery.display_name", config_.display_name); - node_->declare_parameter("ros2_medkit.discovery.component_id", config_.component_id); - node_->declare_parameter("ros2_medkit.discovery.function_ids", config_.function_ids); - node_->declare_parameter( - "ros2_medkit.discovery.process_id", static_cast(pid_)); - node_->declare_parameter("ros2_medkit.discovery.process_name", node_->get_name()); - - if (!hostname_.empty()) { - node_->declare_parameter("ros2_medkit.discovery.hostname", hostname_); - } - - for (const auto & [key, value] : config_.metadata) { - node_->declare_parameter("ros2_medkit.discovery.metadata." + key, value); - } - - RCLCPP_INFO( - node_->get_logger(), "Parameter beacon enabled (entity: %s)", - config_.entity_id.c_str()); - } - - void publish_beacon() - { - auto msg = ros2_medkit_msgs::msg::MedkitDiscoveryHint(); - msg.entity_id = config_.entity_id; - msg.display_name = config_.display_name; - msg.component_id = config_.component_id; - msg.function_ids = config_.function_ids; - msg.process_id = pid_; - msg.process_name = node_->get_name(); - msg.hostname = hostname_; - msg.stamp = node_->now(); - - for (const auto & [key, value] : config_.metadata) { - diagnostic_msgs::msg::KeyValue kv; - kv.key = key; - kv.value = value; - msg.metadata.push_back(kv); - } - - beacon_pub_->publish(msg); - } + void init_topic_beacon(); + void init_param_beacon(); + void publish_beacon(); rclcpp::Node * node_; Config config_; diff --git a/demos/sensor_diagnostics/src/camera_sim_node.cpp b/demos/sensor_diagnostics/src/camera_sim_node.cpp index bfa5ca3..4303ff8 100644 --- a/demos/sensor_diagnostics/src/camera_sim_node.cpp +++ b/demos/sensor_diagnostics/src/camera_sim_node.cpp @@ -74,8 +74,9 @@ class CameraSimNode : public rclcpp::Node std::bind(&CameraSimNode::on_parameter_change, this, std::placeholders::_1)); // Initialize beacon (reads beacon_mode parameter: none/topic/param) + auto entity_id = this->declare_parameter("entity_id", "camera-sim"); beacon_ = std::make_unique(this, BeaconHelper::Config{ - "camera-sim", "Camera Simulator", "camera-unit", + entity_id, "Camera Simulator", "camera-unit", {"sensor-monitoring"}, {{"sensor_type", "camera"}, {"data_topic", "image_raw"}, {"frame_id", "camera_link"}}, }); diff --git a/demos/sensor_diagnostics/src/gps_sim_node.cpp b/demos/sensor_diagnostics/src/gps_sim_node.cpp index c96663b..fd939af 100644 --- a/demos/sensor_diagnostics/src/gps_sim_node.cpp +++ b/demos/sensor_diagnostics/src/gps_sim_node.cpp @@ -74,8 +74,9 @@ class GpsSimNode : public rclcpp::Node std::bind(&GpsSimNode::on_parameter_change, this, std::placeholders::_1)); // Initialize beacon (reads beacon_mode parameter: none/topic/param) + auto entity_id = this->declare_parameter("entity_id", "gps-sim"); beacon_ = std::make_unique(this, BeaconHelper::Config{ - "gps-sim", "GPS Simulator", "gps-unit", + entity_id, "GPS Simulator", "gps-unit", {"sensor-monitoring"}, {{"sensor_type", "gps"}, {"data_topic", "fix"}, {"frame_id", "gps_link"}}, }); diff --git a/demos/sensor_diagnostics/src/imu_sim_node.cpp b/demos/sensor_diagnostics/src/imu_sim_node.cpp index 16ccba9..372478b 100644 --- a/demos/sensor_diagnostics/src/imu_sim_node.cpp +++ b/demos/sensor_diagnostics/src/imu_sim_node.cpp @@ -70,8 +70,9 @@ class ImuSimNode : public rclcpp::Node std::bind(&ImuSimNode::on_parameter_change, this, std::placeholders::_1)); // Initialize beacon (reads beacon_mode parameter: none/topic/param) + auto entity_id = this->declare_parameter("entity_id", "imu-sim"); beacon_ = std::make_unique(this, BeaconHelper::Config{ - "imu-sim", "IMU Simulator", "imu-unit", + entity_id, "IMU Simulator", "imu-unit", {"sensor-monitoring"}, {{"sensor_type", "imu"}, {"data_topic", "imu"}, {"frame_id", "imu_link"}}, }); diff --git a/demos/sensor_diagnostics/src/lidar_sim_node.cpp b/demos/sensor_diagnostics/src/lidar_sim_node.cpp index ddd4762..88cf3d6 100644 --- a/demos/sensor_diagnostics/src/lidar_sim_node.cpp +++ b/demos/sensor_diagnostics/src/lidar_sim_node.cpp @@ -83,8 +83,9 @@ class LidarSimNode : public rclcpp::Node std::bind(&LidarSimNode::on_parameter_change, this, std::placeholders::_1)); // Initialize beacon (reads beacon_mode parameter: none/topic/param) + auto entity_id = this->declare_parameter("entity_id", "lidar-sim"); beacon_ = std::make_unique(this, BeaconHelper::Config{ - "lidar-sim", "LiDAR Simulator", "lidar-unit", + entity_id, "LiDAR Simulator", "lidar-unit", {"sensor-monitoring"}, {{"sensor_type", "lidar"}, {"data_topic", "scan"}, {"frame_id", "lidar_link"}}, });