From 8a66a2e11ea41f3e14120e093cc3a3fce8b48469 Mon Sep 17 00:00:00 2001
From: Chris Lalancette <clalancette@gmail.com>
Date: Wed, 30 Oct 2024 14:19:43 -0400
Subject: [PATCH] feat(config): make parameter 'enabled' dynamic (#548)

* feat(config): make parameter 'enabled' dynamic

So that it can be used no run-time for enabling / disabling the driver.

Signed-off-by: Chris Lalancette <clalancette@gmail.com>
Co-authored-by: Rein Appeldoorn <rein.appeldoorn@nobleo.nl>
---
 velodyne_driver/include/velodyne_driver/driver.hpp | 6 +++++-
 velodyne_driver/src/driver/driver.cpp              | 8 ++++++--
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/velodyne_driver/include/velodyne_driver/driver.hpp b/velodyne_driver/include/velodyne_driver/driver.hpp
index 000ac5811..706107005 100644
--- a/velodyne_driver/include/velodyne_driver/driver.hpp
+++ b/velodyne_driver/include/velodyne_driver/driver.hpp
@@ -33,6 +33,7 @@
 #ifndef VELODYNE_DRIVER__DRIVER_HPP_
 #define VELODYNE_DRIVER__DRIVER_HPP_
 
+#include <atomic>
 #include <future>
 #include <memory>
 #include <string>
@@ -70,7 +71,7 @@ class VelodyneDriver final : public rclcpp::Node
     double rpm;                      // device rotation rate (RPMs)
     int cut_angle;                   // cutting angle in radians
     double time_offset;              // time in seconds added to each velodyne time stamp
-    bool enabled;                    // polling is enabled
+    std::atomic<bool> enabled;       // polling is enabled
     bool timestamp_first_packet;     // timestamp based on first packet instead of last one
   }
   config_;
@@ -85,6 +86,9 @@ class VelodyneDriver final : public rclcpp::Node
   double diag_max_freq_;
   std::unique_ptr<diagnostic_updater::TopicDiagnostic> diag_topic_;
 
+  std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> param_enabled_cb_handle_;
+
   // We use this future/promise pair to notify threads that we are shutting down
   std::shared_future<void> future_;
   std::promise<void> exit_signal_;
diff --git a/velodyne_driver/src/driver/driver.cpp b/velodyne_driver/src/driver/driver.cpp
index dd1402987..5e1bae4da 100644
--- a/velodyne_driver/src/driver/driver.cpp
+++ b/velodyne_driver/src/driver/driver.cpp
@@ -71,7 +71,7 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options)
   offset_desc.floating_point_range.push_back(offset_range);
   config_.time_offset = this->declare_parameter("time_offset", 0.0, offset_desc);
 
-  config_.enabled = this->declare_parameter("enabled", true);
+  config_.enabled.store(this->declare_parameter("enabled", true));
   bool read_once = this->declare_parameter("read_once", false);
   bool read_fast = this->declare_parameter("read_fast", false);
   double repeat_delay = this->declare_parameter("repeat_delay", 0.0);
@@ -83,6 +83,10 @@ VelodyneDriver::VelodyneDriver(const rclcpp::NodeOptions & options)
   int udp_port = this->declare_parameter("port", static_cast<int>(DATA_PORT_NUMBER));
   config_.timestamp_first_packet = this->declare_parameter("timestamp_first_packet", false);
 
+  param_subscriber_ = std::make_shared<rclcpp::ParameterEventHandler>(this);
+  param_enabled_cb_handle_ = param_subscriber_->add_parameter_callback(
+    "enabled", [this](const rclcpp::Parameter & p) {this->config_.enabled.store(p.as_bool());});
+
   future_ = exit_signal_.get_future();
 
   // get model name, validate string, determine packet rate
@@ -196,7 +200,7 @@ VelodyneDriver::~VelodyneDriver()
  */
 bool VelodyneDriver::poll()
 {
-  if (!config_.enabled) {
+  if (!config_.enabled.load()) {
     // If we are not enabled exit once a second to let the caller handle
     // anything it might need to, such as if it needs to exit.
     std::this_thread::sleep_for(std::chrono::seconds(1));