From 17afb513b10528ba9654f71cb29ebfa7b59d7a75 Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Wed, 5 Feb 2025 23:44:39 +0100 Subject: [PATCH 1/7] Make launch_hw param reconfigurable. --- .../velodyne/velodyne_ros_wrapper.hpp | 25 +++ .../src/velodyne/velodyne_ros_wrapper.cpp | 160 +++++++++++++++--- 2 files changed, 157 insertions(+), 28 deletions(-) diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 9f7fe8758..a810e47b8 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -69,6 +69,22 @@ class VelodyneRosWrapper final : public rclcpp::Node Status declare_and_get_sensor_config_params(); + void reconfigure_hw_interface(); + + void create_packet_subscriber(); + void reset_packet_subscriber(); + + // HW interface management + void bringup_hw(bool); + void cleanup_on_hw_reconfigure(); + void setup_on_hw_reconfigure(); + + // Decoder thread management + void configure_decoder_wrapper_thread(); + void start_decoder_thread(); + void stop_decoder_thread(); + void exit_decoder_thread(); + /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult @@ -90,10 +106,19 @@ class VelodyneRosWrapper final : public rclcpp::Node rclcpp::Subscription::SharedPtr packets_sub_{}; bool launch_hw_; + bool use_udp_only_; + + std::mutex decoder_thread_mutex_; + std::condition_variable decoder_thread_cv_; + bool decoder_thread_running_ = false; + bool exit_decoder_thread_ = false; + bool restart_hw_ = false; + bool restart_packet_subscriber_ = false; std::optional hw_interface_wrapper_; std::optional hw_monitor_wrapper_; std::optional decoder_wrapper_; + rclcpp::TimerBase::SharedPtr hw_reconfigure_timer_; std::mutex mtx_config_; diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index dc415d219..bd2e4691f 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -22,7 +22,9 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) packet_queue_(3000), hw_interface_wrapper_(), hw_monitor_wrapper_(), - decoder_wrapper_() + decoder_wrapper_(), + hw_reconfigure_timer_(this->create_wall_timer( + std::chrono::seconds(1), std::bind(&VelodyneRosWrapper::reconfigure_hw_interface, this))) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -34,10 +36,10 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_INFO_STREAM(get_logger(), "Sensor Configuration: " << *sensor_cfg_ptr_); - launch_hw_ = declare_parameter("launch_hw", param_read_only()); - bool use_udp_only = declare_parameter("udp_only", param_read_only()); + launch_hw_ = declare_parameter("launch_hw", param_read_write()); + use_udp_only_ = declare_parameter("udp_only", param_read_only()); - if (use_udp_only) { + if (use_udp_only_) { RCLCPP_INFO_STREAM( get_logger(), "UDP-only mode is enabled. Settings checks, synchronization, and diagnostics publishing are " @@ -45,40 +47,98 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) } if (launch_hw_) { - hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, use_udp_only); - if (!use_udp_only) { // hardware monitor requires HTTP connection - hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); - } + bringup_hw(use_udp_only_); + } else { + create_packet_subscriber(); + } + + decoder_thread_ = std::thread(&VelodyneRosWrapper::configure_decoder_wrapper_thread, this); + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&VelodyneRosWrapper::on_parameter_change, this, std::placeholders::_1)); +} + +void VelodyneRosWrapper::reconfigure_hw_interface() +{ + if (restart_hw_) { + bringup_hw(use_udp_only_); + setup_on_hw_reconfigure(); + restart_hw_ = false; } + if (restart_packet_subscriber_) { + create_packet_subscriber(); + setup_on_hw_reconfigure(); + restart_packet_subscriber_ = false; + } +} + +void VelodyneRosWrapper::create_packet_subscriber() +{ + packets_sub_ = create_subscription( + "velodyne_packets", rclcpp::SensorDataQoS(), + std::bind(&VelodyneRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); +} + +void VelodyneRosWrapper::configure_decoder_wrapper_thread() +{ decoder_wrapper_.emplace( this, hw_interface_wrapper_ ? hw_interface_wrapper_->hw_interface() : nullptr, sensor_cfg_ptr_); RCLCPP_DEBUG(get_logger(), "Starting stream"); - decoder_thread_ = std::thread([this]() { - while (true) { - decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); + while (true) { + std::unique_lock lock(decoder_thread_mutex_); + decoder_thread_cv_.wait( + lock, [this] { return decoder_thread_running_ || exit_decoder_thread_; }); + + if (exit_decoder_thread_) { + break; } - }); + decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); - if (launch_hw_) { - hw_interface_wrapper_->hw_interface()->register_scan_callback( - std::bind(&VelodyneRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); - stream_start(); - } else { - packets_sub_ = create_subscription( - "velodyne_packets", rclcpp::SensorDataQoS(), - std::bind(&VelodyneRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); - RCLCPP_INFO_STREAM( - get_logger(), - "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + if (!decoder_thread_running_) { + continue; + } } +} - // Register parameter callback after all params have been declared. Otherwise it would be called - // once for each declaration - parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&VelodyneRosWrapper::on_parameter_change, this, std::placeholders::_1)); +void VelodyneRosWrapper::start_decoder_thread() +{ + std::scoped_lock lock(decoder_thread_mutex_); + decoder_thread_running_ = true; + decoder_thread_cv_.notify_one(); +} + +void VelodyneRosWrapper::stop_decoder_thread() +{ + std::scoped_lock lock(decoder_thread_mutex_); + decoder_thread_running_ = false; + decoder_thread_cv_.notify_one(); +} + +void VelodyneRosWrapper::exit_decoder_thread() +{ + std::scoped_lock lock(decoder_thread_mutex_); + exit_decoder_thread_ = true; + decoder_thread_cv_.notify_one(); +} + +void VelodyneRosWrapper::bringup_hw(bool use_udp_only) +{ + hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, use_udp_only); + + if (!use_udp_only) { // hardware monitor requires HTTP connection + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); + } + hw_interface_wrapper_->hw_interface()->register_scan_callback( + std::bind(&VelodyneRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); + stream_start(); } nebula::Status VelodyneRosWrapper::declare_and_get_sensor_config_params() @@ -194,6 +254,35 @@ Status VelodyneRosWrapper::stream_start() return hw_interface_wrapper_->hw_interface()->sensor_interface_start(); } +void VelodyneRosWrapper::cleanup_on_hw_reconfigure() +{ + stop_decoder_thread(); + exit_decoder_thread(); + decoder_thread_.join(); + if (hw_interface_wrapper_) { + hw_interface_wrapper_.reset(); + } + if (hw_monitor_wrapper_) { + hw_monitor_wrapper_.reset(); + } + if (decoder_wrapper_) { + decoder_wrapper_.reset(); + } +} + +void VelodyneRosWrapper::setup_on_hw_reconfigure() +{ + decoder_thread_ = std::thread(&VelodyneRosWrapper::configure_decoder_wrapper_thread, this); + start_decoder_thread(); +} + +void VelodyneRosWrapper::reset_packet_subscriber() +{ + if (packets_sub_) { + packets_sub_.reset(); + } +} + rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change( const std::vector & p) { @@ -216,7 +305,22 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change get_param(p, "max_range", new_cfg.max_range) | get_param(p, "rotation_speed", new_cfg.rotation_speed) | get_param(p, "cloud_min_angle", new_cfg.cloud_min_angle) | - get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle); + get_param(p, "cloud_max_angle", new_cfg.cloud_max_angle) | + get_param(p, "launch_hw", launch_hw_); + + if (got_any && launch_hw_ && !hw_interface_wrapper_) { + RCLCPP_INFO(get_logger(), "(re)Configuring HW interface"); + reset_packet_subscriber(); + cleanup_on_hw_reconfigure(); + restart_hw_ = true; + } + + if (got_any && !launch_hw_ && hw_interface_wrapper_) { + RCLCPP_INFO(get_logger(), "(re) Configuring packet subscriber"); + cleanup_on_hw_reconfigure(); + reset_packet_subscriber(); + restart_packet_subscriber_ = true; + } // Currently, HW interface and monitor wrappers have only read-only parameters, so their update // logic is not implemented From 1bfd5e657717c6c7617ff9495e03c640a022bea1 Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Thu, 6 Feb 2025 11:12:03 +0100 Subject: [PATCH 2/7] Avoid double declaration of parameter. --- nebula_ros/src/velodyne/decoder_wrapper.cpp | 8 ++++++-- nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp | 4 ++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp index 3a86fbe61..140f52dcc 100644 --- a/nebula_ros/src/velodyne/decoder_wrapper.cpp +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -31,8 +31,12 @@ VelodyneDecoderWrapper::VelodyneDecoderWrapper( "VelodyneDecoderWrapper cannot be instantiated without a valid config!"); } - calibration_file_path_ = - parent_node->declare_parameter("calibration_file", param_read_write()); + if (parent_node->has_parameter("calibration_file")) { + calibration_file_path_ = parent_node->get_parameter("calibration_file").as_string(); + } else { + calibration_file_path_ = + parent_node->declare_parameter("calibration_file", param_read_write()); + } auto calibration_result = get_calibration_data(calibration_file_path_); if (!calibration_result.has_value()) { diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index bd2e4691f..650997882 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -66,12 +66,14 @@ void VelodyneRosWrapper::reconfigure_hw_interface() bringup_hw(use_udp_only_); setup_on_hw_reconfigure(); restart_hw_ = false; + hw_reconfigure_timer_->cancel(); } if (restart_packet_subscriber_) { create_packet_subscriber(); setup_on_hw_reconfigure(); restart_packet_subscriber_ = false; + hw_reconfigure_timer_->cancel(); } } @@ -313,6 +315,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change reset_packet_subscriber(); cleanup_on_hw_reconfigure(); restart_hw_ = true; + hw_reconfigure_timer_->reset(); } if (got_any && !launch_hw_ && hw_interface_wrapper_) { @@ -320,6 +323,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change cleanup_on_hw_reconfigure(); reset_packet_subscriber(); restart_packet_subscriber_ = true; + hw_reconfigure_timer_->reset(); } // Currently, HW interface and monitor wrappers have only read-only parameters, so their update From c82294bafa076e5f1239f46d583c13c5de4dac33 Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Thu, 6 Feb 2025 18:04:52 +0100 Subject: [PATCH 3/7] Initial working version for velodyne launch_hw reconfig. Signed-off-by: Mukunda Bharatheesha --- nebula_ros/CMakeLists.txt | 2 +- .../velodyne/velodyne_ros_wrapper.hpp | 13 ++-- .../src/velodyne/hw_interface_wrapper.cpp | 7 ++- .../src/velodyne/hw_monitor_wrapper.cpp | 14 ++++- .../src/velodyne/velodyne_ros_wrapper.cpp | 59 +++++-------------- 5 files changed, 36 insertions(+), 59 deletions(-) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index aafb8d96a..8e52b11e4 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -3,7 +3,7 @@ project(nebula_ros) # Default to C++17 if (NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD 20) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index a810e47b8..076e56870 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -80,10 +80,9 @@ class VelodyneRosWrapper final : public rclcpp::Node void setup_on_hw_reconfigure(); // Decoder thread management - void configure_decoder_wrapper_thread(); - void start_decoder_thread(); - void stop_decoder_thread(); - void exit_decoder_thread(); + void decoder_wrapper_thread(std::stop_token stoken); + void set_decoder_wrapper(); + void stop_decoder_thread() { decoder_thread_.request_stop(); } /// @brief rclcpp parameter callback /// @param parameters Received parameters @@ -101,17 +100,13 @@ class VelodyneRosWrapper final : public rclcpp::Node /// @brief Stores received packets that have not been processed yet by the decoder thread MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving - std::thread decoder_thread_; + std::jthread decoder_thread_; rclcpp::Subscription::SharedPtr packets_sub_{}; bool launch_hw_; bool use_udp_only_; - std::mutex decoder_thread_mutex_; - std::condition_variable decoder_thread_cv_; - bool decoder_thread_running_ = false; - bool exit_decoder_thread_ = false; bool restart_hw_ = false; bool restart_packet_subscriber_ = false; diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp index 4e2b7202d..a04d9eb73 100644 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -17,8 +17,11 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( status_(Status::NOT_INITIALIZED), use_udp_only_(use_udp_only) { - setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); - + if (parent_node->has_parameter("setup_sensor")) { + setup_sensor_ = parent_node->get_parameter("setup_sensor").as_bool(); + } else { + setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); + } hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); status_ = hw_interface_->initialize_sensor_configuration(config); diff --git a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp index 3c4179802..c8c59a2ed 100644 --- a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp @@ -23,9 +23,17 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper( parent_node_(parent_node), sensor_configuration_(config) { - diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); - show_advanced_diagnostics_ = - parent_node->declare_parameter("advanced_diagnostics", param_read_only()); + if (parent_node->has_parameter("diag_span")) { + diag_span_ = parent_node->get_parameter("diag_span").as_int(); + } else { + diag_span_ = parent_node->declare_parameter("diag_span", param_read_only()); + } + if (parent_node->has_parameter("advanced_diagnostics")) { + show_advanced_diagnostics_ = parent_node->get_parameter("advanced_diagnostics").as_bool(); + } else { + show_advanced_diagnostics_ = + parent_node->declare_parameter("advanced_diagnostics", param_read_only()); + } std::cout << "Get model name and serial." << std::endl; auto str = hw_interface_->get_snapshot(); diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index 650997882..51679f31e 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -26,6 +26,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) hw_reconfigure_timer_(this->create_wall_timer( std::chrono::seconds(1), std::bind(&VelodyneRosWrapper::reconfigure_hw_interface, this))) { + hw_reconfigure_timer_->cancel(); // Cancel the timer until the HW interface is initialized setvbuf(stdout, NULL, _IONBF, BUFSIZ); wrapper_status_ = declare_and_get_sensor_config_params(); @@ -52,7 +53,8 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) create_packet_subscriber(); } - decoder_thread_ = std::thread(&VelodyneRosWrapper::configure_decoder_wrapper_thread, this); + set_decoder_wrapper(); + decoder_thread_ = std::jthread(&VelodyneRosWrapper::decoder_wrapper_thread, this); // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration @@ -87,50 +89,21 @@ void VelodyneRosWrapper::create_packet_subscriber() "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); } -void VelodyneRosWrapper::configure_decoder_wrapper_thread() +void VelodyneRosWrapper::set_decoder_wrapper() { decoder_wrapper_.emplace( this, hw_interface_wrapper_ ? hw_interface_wrapper_->hw_interface() : nullptr, sensor_cfg_ptr_); +} +void VelodyneRosWrapper::decoder_wrapper_thread(std::stop_token stoken) +{ RCLCPP_DEBUG(get_logger(), "Starting stream"); - while (true) { - std::unique_lock lock(decoder_thread_mutex_); - decoder_thread_cv_.wait( - lock, [this] { return decoder_thread_running_ || exit_decoder_thread_; }); - - if (exit_decoder_thread_) { - break; - } + while (!stoken.stop_requested()) { decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); - - if (!decoder_thread_running_) { - continue; - } } } -void VelodyneRosWrapper::start_decoder_thread() -{ - std::scoped_lock lock(decoder_thread_mutex_); - decoder_thread_running_ = true; - decoder_thread_cv_.notify_one(); -} - -void VelodyneRosWrapper::stop_decoder_thread() -{ - std::scoped_lock lock(decoder_thread_mutex_); - decoder_thread_running_ = false; - decoder_thread_cv_.notify_one(); -} - -void VelodyneRosWrapper::exit_decoder_thread() -{ - std::scoped_lock lock(decoder_thread_mutex_); - exit_decoder_thread_ = true; - decoder_thread_cv_.notify_one(); -} - void VelodyneRosWrapper::bringup_hw(bool use_udp_only) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_, use_udp_only); @@ -259,23 +232,18 @@ Status VelodyneRosWrapper::stream_start() void VelodyneRosWrapper::cleanup_on_hw_reconfigure() { stop_decoder_thread(); - exit_decoder_thread(); - decoder_thread_.join(); if (hw_interface_wrapper_) { hw_interface_wrapper_.reset(); } if (hw_monitor_wrapper_) { hw_monitor_wrapper_.reset(); } - if (decoder_wrapper_) { - decoder_wrapper_.reset(); - } } void VelodyneRosWrapper::setup_on_hw_reconfigure() { - decoder_thread_ = std::thread(&VelodyneRosWrapper::configure_decoder_wrapper_thread, this); - start_decoder_thread(); + set_decoder_wrapper(); + decoder_thread_ = std::jthread(&VelodyneRosWrapper::decoder_wrapper_thread, this); } void VelodyneRosWrapper::reset_packet_subscriber() @@ -311,17 +279,20 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change get_param(p, "launch_hw", launch_hw_); if (got_any && launch_hw_ && !hw_interface_wrapper_) { - RCLCPP_INFO(get_logger(), "(re)Configuring HW interface"); + RCLCPP_INFO(get_logger(), "Cancelling packet subscription..."); reset_packet_subscriber(); + RCLCPP_INFO(get_logger(), "Resetting HW interface and HW Monitor wrappers..."); cleanup_on_hw_reconfigure(); + RCLCPP_INFO(get_logger(), "(re)Configuring HW interface"); restart_hw_ = true; hw_reconfigure_timer_->reset(); } if (got_any && !launch_hw_ && hw_interface_wrapper_) { - RCLCPP_INFO(get_logger(), "(re) Configuring packet subscriber"); + RCLCPP_INFO(get_logger(), "Resetting HW interface and HW Monitor wrappers..."); cleanup_on_hw_reconfigure(); reset_packet_subscriber(); + RCLCPP_INFO(get_logger(), "(re) Configuring packet subscriber"); restart_packet_subscriber_ = true; hw_reconfigure_timer_->reset(); } From 408a407355c92e080407622213b9f42f04936261 Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Thu, 6 Feb 2025 18:10:23 +0100 Subject: [PATCH 4/7] Fix deprecation warnings in mt queue for cpp 20. Signed-off-by: Mukunda Bharatheesha --- nebula_ros/include/nebula_ros/common/mt_queue.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp index 40f26253d..20d26e814 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -46,7 +46,7 @@ class MtQueue void push(T && value) { std::unique_lock lock(this->mutex_); - this->cv_not_full_.wait(lock, [=] { return this->queue_.size() < this->capacity_; }); + this->cv_not_full_.wait(lock, [this] { return this->queue_.size() < this->capacity_; }); queue_.push_front(std::move(value)); this->cv_not_empty_.notify_all(); } @@ -54,7 +54,7 @@ class MtQueue T pop() { std::unique_lock lock(this->mutex_); - this->cv_not_empty_.wait(lock, [=] { return !this->queue_.empty(); }); + this->cv_not_empty_.wait(lock, [this] { return !this->queue_.empty(); }); T return_value(std::move(this->queue_.back())); this->queue_.pop_back(); this->cv_not_full_.notify_all(); From dcae7330bdf95dacfa99561a02e2471d8a26f8dc Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Sat, 8 Feb 2025 16:48:28 +0100 Subject: [PATCH 5/7] Improve decoder thread mgmt on launch_hw reconfig. Signed-off-by: Mukunda Bharatheesha --- .../include/nebula_ros/common/mt_queue.hpp | 13 +++++++ .../velodyne/velodyne_ros_wrapper.hpp | 2 +- .../src/velodyne/velodyne_ros_wrapper.cpp | 35 ++++++++++++++----- 3 files changed, 41 insertions(+), 9 deletions(-) diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp index 20d26e814..46125b518 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -61,4 +61,17 @@ class MtQueue return return_value; } + + std::pair pop(std::chrono::milliseconds timeout) + { + std::unique_lock lock(this->mutex_); + if (!this->cv_not_empty_.wait_for(lock, timeout, [this] { return !this->queue_.empty(); })) { + return std::make_pair(T(), false); + } + T return_value(std::move(this->queue_.back())); + this->queue_.pop_back(); + this->cv_not_full_.notify_all(); + + return std::make_pair(std::move(return_value), true); + } }; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 076e56870..0943539ab 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -77,7 +77,7 @@ class VelodyneRosWrapper final : public rclcpp::Node // HW interface management void bringup_hw(bool); void cleanup_on_hw_reconfigure(); - void setup_on_hw_reconfigure(); + void setup_decoder(); // Decoder thread management void decoder_wrapper_thread(std::stop_token stoken); diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index 51679f31e..bd0d08000 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -4,6 +4,8 @@ #include +#include // debug only + #include #include #include @@ -26,7 +28,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) hw_reconfigure_timer_(this->create_wall_timer( std::chrono::seconds(1), std::bind(&VelodyneRosWrapper::reconfigure_hw_interface, this))) { - hw_reconfigure_timer_->cancel(); // Cancel the timer until the HW interface is initialized + hw_reconfigure_timer_->cancel(); setvbuf(stdout, NULL, _IONBF, BUFSIZ); wrapper_status_ = declare_and_get_sensor_config_params(); @@ -53,8 +55,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) create_packet_subscriber(); } - set_decoder_wrapper(); - decoder_thread_ = std::jthread(&VelodyneRosWrapper::decoder_wrapper_thread, this); + setup_decoder(); // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration @@ -66,14 +67,14 @@ void VelodyneRosWrapper::reconfigure_hw_interface() { if (restart_hw_) { bringup_hw(use_udp_only_); - setup_on_hw_reconfigure(); + setup_decoder(); restart_hw_ = false; hw_reconfigure_timer_->cancel(); } if (restart_packet_subscriber_) { create_packet_subscriber(); - setup_on_hw_reconfigure(); + setup_decoder(); restart_packet_subscriber_ = false; hw_reconfigure_timer_->cancel(); } @@ -91,6 +92,10 @@ void VelodyneRosWrapper::create_packet_subscriber() void VelodyneRosWrapper::set_decoder_wrapper() { + // If there's an existing decoder wrapper, reset it + if (decoder_wrapper_) { + decoder_wrapper_.reset(); + } decoder_wrapper_.emplace( this, hw_interface_wrapper_ ? hw_interface_wrapper_->hw_interface() : nullptr, sensor_cfg_ptr_); } @@ -100,8 +105,14 @@ void VelodyneRosWrapper::decoder_wrapper_thread(std::stop_token stoken) RCLCPP_DEBUG(get_logger(), "Starting stream"); while (!stoken.stop_requested()) { - decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); + auto [packet, valid] = packet_queue_.pop(std::chrono::milliseconds(100)); + if (valid) { + decoder_wrapper_->process_cloud_packet(std::move(packet)); + } else { + continue; + } } + RCLCPP_INFO(get_logger(), "Gracefully stopped decoder thread"); } void VelodyneRosWrapper::bringup_hw(bool use_udp_only) @@ -231,7 +242,13 @@ Status VelodyneRosWrapper::stream_start() void VelodyneRosWrapper::cleanup_on_hw_reconfigure() { - stop_decoder_thread(); + while (true) { + if (decoder_thread_.joinable()) { + stop_decoder_thread(); + break; + } + } + decoder_thread_.join(); if (hw_interface_wrapper_) { hw_interface_wrapper_.reset(); } @@ -240,10 +257,12 @@ void VelodyneRosWrapper::cleanup_on_hw_reconfigure() } } -void VelodyneRosWrapper::setup_on_hw_reconfigure() +void VelodyneRosWrapper::setup_decoder() { set_decoder_wrapper(); decoder_thread_ = std::jthread(&VelodyneRosWrapper::decoder_wrapper_thread, this); + // Set the thread name + pthread_setname_np(decoder_thread_.native_handle(), "VelDecThread"); // debug only } void VelodyneRosWrapper::reset_packet_subscriber() From 6858e48188fde3e4478fd30a34297cd11e5c519f Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Tue, 11 Feb 2025 15:18:51 +0100 Subject: [PATCH 6/7] Always wait on hardware if hardware is true. --- .../velodyne_hw_interface.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 1f70ee674..9e28f5e0e 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -107,16 +107,19 @@ Status VelodyneHwInterface::get_sensor_configuration(SensorConfigurationBase & s VelodyneStatus VelodyneHwInterface::init_http_client() { - try { - http_client_driver_->init_client(sensor_configuration_->sensor_ip, 80); - if (!http_client_driver_->client()->isOpen()) { - http_client_driver_->client()->open(); + while (true) { + try { + http_client_driver_->init_client(sensor_configuration_->sensor_ip, 80); + if (!http_client_driver_->client()->isOpen()) { + http_client_driver_->client()->open(); + } + return Status::OK; + } catch (const std::exception & ex) { + std::cerr << "Error initializing lidar: " << ex.what() << std::endl; + std::cerr << "Retrying after 5 seconds..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(5)); } - } catch (const std::exception & ex) { - VelodyneStatus status = Status::HTTP_CONNECTION_ERROR; - return status; } - return Status::OK; } void VelodyneHwInterface::string_callback(const std::string & str) From 0afdec687682d836e44f2fb709bb7fc0b537b7cb Mon Sep 17 00:00:00 2001 From: Mukunda Bharatheesha Date: Tue, 11 Feb 2025 17:08:15 +0100 Subject: [PATCH 7/7] Log hardware connection failure only once. --- .../nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 9e28f5e0e..7382b14a2 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -115,8 +115,9 @@ VelodyneStatus VelodyneHwInterface::init_http_client() } return Status::OK; } catch (const std::exception & ex) { - std::cerr << "Error initializing lidar: " << ex.what() << std::endl; - std::cerr << "Retrying after 5 seconds..." << std::endl; + RCLCPP_INFO_ONCE( + *parent_node_logger_, "Cannot connect to lidar because of: %s. Will keep trying...", + ex.what()); std::this_thread::sleep_for(std::chrono::seconds(5)); } }