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 <[email protected]> Co-authored-by: Rein Appeldoorn <[email protected]>
ros-drivers · Oct 30, 2024 · 8a66a2e · 8a66a2e
1 parent 8a0a663
commit 8a66a2e
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 3 deletions.
diff --git 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
@@ -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));