feat(config): make parameter 'enabled' dynamic

So that it can be used no run-time for enabling / disabling the driver.
nobleo · Oct 1, 2024 · ad7a5fd · ad7a5fd
1 parent 04a7ca7
commit ad7a5fd
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));