diff --git a/nebula_common/include/nebula_common/loggers/logger.hpp b/nebula_common/include/nebula_common/loggers/logger.hpp index b416e252..9bf40715 100644 --- a/nebula_common/include/nebula_common/loggers/logger.hpp +++ b/nebula_common/include/nebula_common/loggers/logger.hpp @@ -31,11 +31,6 @@ namespace nebula::drivers::loggers class Logger { public: - Logger() = default; - Logger(const Logger &) = default; - Logger(Logger &&) = delete; - Logger & operator=(const Logger &) = default; - Logger & operator=(Logger &&) = delete; virtual ~Logger() = default; virtual void debug(const std::string & message) = 0; 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 f8d221c3..8bf0100f 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 73869d1f..dbf5fe09 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 diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index ef558f15..9e4ce884 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -2,10 +2,11 @@ #include "nebula_ros/hesai/hw_interface_wrapper.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/common/rclcpp_logger.hpp" +#include + #include namespace nebula::ros