From 658aab16546a673d6875364addcafeb9a6319d9c Mon Sep 17 00:00:00 2001 From: Max Schmeller <6088931+mojomex@users.noreply.github.com> Date: Mon, 25 Nov 2024 18:52:18 +0900 Subject: [PATCH] perf(hesai): remove decoder thread and MtQueue, make everything single-threaded (#226) Signed-off-by: Max SCHMELLER --- .../include/nebula_ros/hesai/hesai_ros_wrapper.hpp | 6 ------ nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 13 ++----------- 2 files changed, 2 insertions(+), 17 deletions(-) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index f8d221c3f..8bf0100ff 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -17,7 +17,6 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" #include "nebula_ros/hesai/hw_monitor_wrapper.hpp" @@ -98,11 +97,6 @@ class HesaiRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; - /// @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_; - rclcpp::Subscription::SharedPtr packets_sub_{}; bool launch_hw_; diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 73869d1fd..dbf5fe099 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -23,7 +23,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), sensor_cfg_ptr_(nullptr), - packet_queue_(3000), hw_interface_wrapper_(), hw_monitor_wrapper_(), decoder_wrapper_() @@ -82,12 +81,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_DEBUG(get_logger(), "Starting stream"); - decoder_thread_ = std::thread([this]() { - while (true) { - decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); - } - }); - if (launch_hw_) { hw_interface_wrapper_->hw_interface()->RegisterScanCallback( std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); @@ -289,7 +282,7 @@ void HesaiRosWrapper::receive_scan_message_callback( nebula_pkt_ptr->stamp = pkt.stamp; std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); - packet_queue_.push(std::move(nebula_pkt_ptr)); + decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr)); } } @@ -428,9 +421,7 @@ void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packe msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } + decoder_wrapper_->process_cloud_packet(std::move(msg_ptr)); } std::string HesaiRosWrapper::get_calibration_parameter_name(drivers::SensorModel model) const