From 6f9a39fb43953fa38d4debc39693d24afa683320 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 20 Feb 2025 11:56:18 +0900 Subject: [PATCH 1/3] chore(ars548): remove MtQueue and decoder thread Signed-off-by: Max SCHMELLER --- .../continental_ars548_ros_wrapper.hpp | 5 ----- .../continental_ars548_ros_wrapper.cpp | 17 +++-------------- 2 files changed, 3 insertions(+), 19 deletions(-) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 1d755a3ef..64e7f074c 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -80,11 +80,6 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node std::shared_ptr config_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/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 0cf61e9d1..0528119a3 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -29,10 +29,7 @@ namespace nebula::ros ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node( "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), - wrapper_status_(Status::NOT_INITIALIZED), - packet_queue_(3000), - hw_interface_wrapper_(), - decoder_wrapper_() + wrapper_status_(Status::NOT_INITIALIZED) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -54,12 +51,6 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio RCLCPP_DEBUG(get_logger(), "Starting stream"); - decoder_thread_ = std::thread([this]() { - while (true) { - decoder_wrapper_->process_packet(packet_queue_.pop()); - } - }); - if (launch_hw_) { hw_interface_wrapper_->hw_interface()->register_packet_callback(std::bind( &ContinentalARS548RosWrapper::receive_packet_callback, this, std::placeholders::_1)); @@ -155,7 +146,7 @@ void ContinentalARS548RosWrapper::receive_packets_callback( nebula_packet_ptr->stamp = packet.stamp; nebula_packet_ptr->data = std::move(packet.data); - packet_queue_.push(std::move(nebula_packet_ptr)); + decoder_wrapper_->process_packet(std::move(nebula_packet_ptr)); } } @@ -166,9 +157,7 @@ void ContinentalARS548RosWrapper::receive_packet_callback( return; } - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); - } + decoder_wrapper_->process_packet(std::move(msg_ptr)); } Status ContinentalARS548RosWrapper::get_status() From b2bf082239221e5bbdce0ed4b594aa3f431049f9 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 20 Feb 2025 11:56:32 +0900 Subject: [PATCH 2/3] test(ars548): add now-passing smoke test Signed-off-by: Max SCHMELLER --- nebula_ros/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index e4399af41..7060f6f56 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -228,7 +228,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() - foreach(MODEL Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT16 PandarXT32 PandarXT32M) + foreach(MODEL Pandar40P Pandar64 PandarQT64 PandarQT128 Pandar128E4X PandarAT128 PandarXT16 PandarXT32 PandarXT32M ARS548) string(TOLOWER ${MODEL}_smoke_test test_name) add_ros_test( test/smoke_test.py From c7fc7341270287f0c98a8c38dabeee7e78c19b54 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Thu, 20 Feb 2025 12:26:02 +0900 Subject: [PATCH 3/3] chore(ars548): remove unused mtqueue import Signed-off-by: Max SCHMELLER --- .../nebula_ros/continental/continental_ars548_ros_wrapper.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 64e7f074c..7e242b0ef 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,7 +14,6 @@ #pragma once -#include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp"