diff --git a/demos/sensor_diagnostics/CMakeLists.txt b/demos/sensor_diagnostics/CMakeLists.txt index 09144bc..c124333 100644 --- a/demos/sensor_diagnostics/CMakeLists.txt +++ b/demos/sensor_diagnostics/CMakeLists.txt @@ -15,8 +15,20 @@ 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 @@ -26,6 +38,7 @@ ament_target_dependencies(lidar_sim_node # 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 @@ -34,6 +47,7 @@ ament_target_dependencies(imu_sim_node # 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 @@ -42,6 +56,7 @@ ament_target_dependencies(gps_sim_node # 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 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..a09019f 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=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..16057e1 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 @@ -19,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 @@ -50,6 +56,18 @@ 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').strip().lower() + valid_beacon_modes = ('none', 'topic', 'param') + if beacon_mode not in valid_beacon_modes: + 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( 'ros2_medkit_graph_provider', 'ros2_medkit_graph_provider_plugin') @@ -65,8 +83,46 @@ 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'] = 15.0 + else: + 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') + 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 + else: + 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 + # 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 +132,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 +143,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 +155,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 +180,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 +232,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.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 new file mode 100644 index 0000000..3e21d63 --- /dev/null +++ b/demos/sensor_diagnostics/src/beacon_helper.hpp @@ -0,0 +1,53 @@ +// 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 "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::map metadata; + }; + + BeaconHelper(rclcpp::Node * node, const Config & config); + +private: + void init_topic_beacon(); + void init_param_beacon(); + void publish_beacon(); + + 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_; +}; + +} // 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..4303ff8 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,14 @@ 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) + auto entity_id = this->declare_parameter("entity_id", "camera-sim"); + beacon_ = std::make_unique(this, BeaconHelper::Config{ + entity_id, "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 +268,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..fd939af 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,14 @@ 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) + auto entity_id = this->declare_parameter("entity_id", "gps-sim"); + beacon_ = std::make_unique(this, BeaconHelper::Config{ + entity_id, "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 +252,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..372478b 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,14 @@ 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) + auto entity_id = this->declare_parameter("entity_id", "imu-sim"); + beacon_ = std::make_unique(this, BeaconHelper::Config{ + entity_id, "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 +230,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..88cf3d6 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,14 @@ 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) + auto entity_id = this->declare_parameter("entity_id", "lidar-sim"); + beacon_ = std::make_unique(this, BeaconHelper::Config{ + entity_id, "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 +319,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};