From 1fa7e617d5a049f1075f8576414ea4b680664c83 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Thu, 23 Jul 2026 13:48:29 +0900 Subject: [PATCH 1/9] feat(nebula_core_ros): make shared diagnostics/sync helpers agnocast_wrapper::Node-compatible Signed-off-by: Koichi Imai --- .../sync_tooling/sync_tooling_worker.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp index 7569aed7c..e6ccde345 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp @@ -14,6 +14,9 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" + #include #include #include @@ -63,13 +66,10 @@ inline ClockId make_sensor_clock_id(const std::string & frame_id) class SyncToolingWorker { public: - template SyncToolingWorker( - NodeT * const parent_node, const std::string & topic, const std::string & frame_id, - uint8_t ptp_domain_id) - : publisher_( - rclcpp::create_publisher( - *parent_node, topic, rclcpp::QoS(10))), + nebula::agnocast_wrapper::Node * const parent_node, const std::string & topic, + const std::string & frame_id, uint8_t ptp_domain_id) + : publisher_(parent_node->create_publisher(topic, 10)), hostname_(get_hostname()), sensor_id_(make_sensor_clock_id(frame_id)), ptp_domain_id_(ptp_domain_id) @@ -172,7 +172,7 @@ class SyncToolingWorker publisher_->publish(ros2_msg); } - rclcpp::Publisher::SharedPtr publisher_; + NEBULA_PUBLISHER_PTR(std_msgs::msg::UInt8MultiArray) publisher_; std::string hostname_; ClockId sensor_id_; From eaedd42e0b9105dc2daf1c402b129a956ecba985 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Thu, 23 Jul 2026 13:50:07 +0900 Subject: [PATCH 2/9] feat(continental_ars548_ros_wrapper): migrate ContinentalARS548RosWrapper to agnocast_wrapper::Node (Method 2) Signed-off-by: Koichi Imai --- .../nebula_continental/CMakeLists.txt | 2 +- .../continental_ars548_decoder_wrapper.hpp | 45 ++++++++++--------- ...ontinental_ars548_hw_interface_wrapper.hpp | 36 ++++++++------- .../continental_ars548_ros_wrapper.hpp | 10 +++-- .../continental_ars548_decoder_wrapper.cpp | 21 ++++++--- ...ontinental_ars548_hw_interface_wrapper.cpp | 8 ++-- .../src/continental_ars548_ros_wrapper.cpp | 11 +++-- 7 files changed, 72 insertions(+), 61 deletions(-) diff --git a/src/nebula_continental/nebula_continental/CMakeLists.txt b/src/nebula_continental/nebula_continental/CMakeLists.txt index 62bf6e40a..87973d269 100644 --- a/src/nebula_continental/nebula_continental/CMakeLists.txt +++ b/src/nebula_continental/nebula_continental/CMakeLists.txt @@ -50,7 +50,7 @@ if("$ENV{ENABLE_AGNOCAST}") agnocast_components_register_node( continental_ars548_ros_wrapper PLUGIN "ContinentalARS548RosWrapper" EXECUTABLE continental_ars548_ros_wrapper_node - EXECUTOR CallbackIsolatedAgnocastExecutor) + EXECUTOR AgnocastOnlyCallbackIsolatedExecutor) else() rclcpp_components_register_node( continental_ars548_ros_wrapper PLUGIN "ContinentalARS548RosWrapper" diff --git a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_decoder_wrapper.hpp b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_decoder_wrapper.hpp index 00ee0941d..668905246 100644 --- a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_decoder_wrapper.hpp +++ b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_decoder_wrapper.hpp @@ -14,6 +14,8 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/diagnostic_updater.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/diagnostics/hysteresis_state_machine.hpp" #include "nebula_core_ros/diagnostics/liveness_monitor.hpp" #include "nebula_core_ros/diagnostics/rate_bound_status.hpp" @@ -66,7 +68,7 @@ class ContinentalARS548DecoderWrapper { public: ContinentalARS548DecoderWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config, bool launch_hw); @@ -130,7 +132,7 @@ class ContinentalARS548DecoderWrapper autoware_sensing_msgs::msg::RadarObjects convert_to_autoware_radar_objects( const continental_msgs::msg::ContinentalArs548ObjectList & msg); - void initialize_sync_diagnostics(rclcpp::Node * parent_node); + void initialize_sync_diagnostics(nebula::agnocast_wrapper::Node * parent_node); /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg @@ -176,7 +178,7 @@ class ContinentalARS548DecoderWrapper /// @param name The name of the rate bound status /// @return RateBoundStatus for OK and WARN diagnostics static custom_diagnostic_tasks::RateBoundStatus make_rate_bound_status( - rclcpp::Node * const node, const std::string & name) + nebula::agnocast_wrapper::Node * const node, const std::string & name) { static constexpr bool immediate_error_report = false; static constexpr bool immediate_relax_state = true; @@ -217,7 +219,7 @@ class ContinentalARS548DecoderWrapper /// @param diagnostics_ns The diagnostics namespace for parameter /// @return HysteresisStateMachine for internal diagnostics static custom_diagnostic_tasks::HysteresisStateMachine make_hysteresis_status( - rclcpp::Node * const node, const std::string & diagnostics_ns) + nebula::agnocast_wrapper::Node * const node, const std::string & diagnostics_ns) { static constexpr bool immediate_error_report = false; static constexpr bool immediate_relax_state = true; @@ -241,13 +243,13 @@ class ContinentalARS548DecoderWrapper custom_diagnostic_tasks::HysteresisStateMachine blockage_status_; custom_diagnostic_tasks::HysteresisStateMachine internal_status_; nebula::ros::LivenessMonitor liveness_monitor_; - diagnostic_updater::Updater objects_diagnostics_updater_; - diagnostic_updater::Updater detections_diagnostics_updater_; - diagnostic_updater::Updater liveness_diagnostics_updater_; + nebula::agnocast_wrapper::diagnostic_updater::Updater objects_diagnostics_updater_; + nebula::agnocast_wrapper::diagnostic_updater::Updater detections_diagnostics_updater_; + nebula::agnocast_wrapper::diagnostic_updater::Updater liveness_diagnostics_updater_; nebula::Status status_; rclcpp::Logger logger_; - rclcpp::Node * const parent_node_; + nebula::agnocast_wrapper::Node * const parent_node_; std::shared_ptr config_ptr_ RCPPUTILS_TSA_GUARDED_BY(mtx_config_ptr_); @@ -258,20 +260,19 @@ class ContinentalARS548DecoderWrapper std::mutex mtx_driver_ptr_; std::shared_mutex mtx_config_ptr_; - rclcpp::Publisher::SharedPtr packets_pub_{}; - - rclcpp::Publisher::SharedPtr - detection_list_pub_{}; - rclcpp::Publisher::SharedPtr - object_list_pub_{}; - rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; - rclcpp::Publisher::SharedPtr detection_pointcloud_pub_{}; - rclcpp::Publisher::SharedPtr autoware_objects_pub_{}; - rclcpp::Publisher::SharedPtr scan_raw_pub_{}; - rclcpp::Publisher::SharedPtr objects_raw_pub_{}; - rclcpp::Publisher::SharedPtr objects_markers_pub_{}; - rclcpp::Publisher::SharedPtr diagnostics_pub_{}; - rclcpp::Publisher::SharedPtr radar_info_pub_{}; + NEBULA_PUBLISHER_PTR(nebula_msgs::msg::NebulaPackets) packets_pub_ {}; + + NEBULA_PUBLISHER_PTR(continental_msgs::msg::ContinentalArs548DetectionList) + detection_list_pub_{}; + NEBULA_PUBLISHER_PTR(continental_msgs::msg::ContinentalArs548ObjectList) object_list_pub_ {}; + NEBULA_PUBLISHER_PTR(sensor_msgs::msg::PointCloud2) object_pointcloud_pub_ {}; + NEBULA_PUBLISHER_PTR(sensor_msgs::msg::PointCloud2) detection_pointcloud_pub_ {}; + NEBULA_PUBLISHER_PTR(autoware_sensing_msgs::msg::RadarObjects) autoware_objects_pub_ {}; + NEBULA_PUBLISHER_PTR(radar_msgs::msg::RadarScan) scan_raw_pub_ {}; + NEBULA_PUBLISHER_PTR(radar_msgs::msg::RadarTracks) objects_raw_pub_ {}; + NEBULA_PUBLISHER_PTR(visualization_msgs::msg::MarkerArray) objects_markers_pub_ {}; + NEBULA_PUBLISHER_PTR(diagnostic_msgs::msg::DiagnosticArray) diagnostics_pub_ {}; + NEBULA_PUBLISHER_PTR(autoware_sensing_msgs::msg::RadarInfo) radar_info_pub_ {}; autoware_sensing_msgs::msg::RadarInfo radar_info_msg_{}; std::size_t detection_msgs_counter_{0}; diff --git a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_hw_interface_wrapper.hpp b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_hw_interface_wrapper.hpp index c2d2312b5..e83e523fe 100644 --- a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_hw_interface_wrapper.hpp +++ b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_hw_interface_wrapper.hpp @@ -14,6 +14,7 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/parameter_descriptors.hpp" #include @@ -37,7 +38,7 @@ class ContinentalARS548HwInterfaceWrapper { public: ContinentalARS548HwInterfaceWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr & config); @@ -57,15 +58,17 @@ class ContinentalARS548HwInterfaceWrapper private: /// @brief Callback to send the odometry information to the radar device /// @param msg The odometry message - void odometry_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void odometry_callback( + NEBULA_MESSAGE_CONST_SHARED_PTR(geometry_msgs::msg::TwistWithCovarianceStamped) msg); /// @brief Callback to send the acceleration information to the radar device /// @param msg The acceleration message - void acceleration_callback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void acceleration_callback( + NEBULA_MESSAGE_CONST_SHARED_PTR(geometry_msgs::msg::AccelWithCovarianceStamped) msg); /// @brief Callback to send the steering angle information to the radar device /// @param msg The steering angle message - void steering_angle_callback(const std_msgs::msg::Float32::SharedPtr msg); + void steering_angle_callback(NEBULA_MESSAGE_CONST_SHARED_PTR(std_msgs::msg::Float32) msg); /// @brief Service callback to set the new sensor ip /// @param request service request @@ -103,17 +106,16 @@ class ContinentalARS548HwInterfaceWrapper const std::shared_ptr response); - rclcpp::Node * const parent_node_; + nebula::agnocast_wrapper::Node * const parent_node_; std::shared_ptr hw_interface_{}; rclcpp::Logger logger_; nebula::Status status_{}; std::shared_ptr config_ptr_{}; - rclcpp::Subscription::SharedPtr odometry_sub_{}; - rclcpp::Subscription::SharedPtr - acceleration_sub_{}; - rclcpp::Subscription::SharedPtr steering_angle_sub_{}; + NEBULA_SUBSCRIPTION_PTR(geometry_msgs::msg::TwistWithCovarianceStamped) odometry_sub_ {}; + NEBULA_SUBSCRIPTION_PTR(geometry_msgs::msg::AccelWithCovarianceStamped) acceleration_sub_ {}; + NEBULA_SUBSCRIPTION_PTR(std_msgs::msg::Float32) steering_angle_sub_ {}; nebula::util::RateChecker odometry_rate_checker_; nebula::util::RateChecker acceleration_rate_checker_; @@ -121,13 +123,13 @@ class ContinentalARS548HwInterfaceWrapper bool standstill_{true}; - rclcpp::Service::SharedPtr - set_network_configuration_service_server_{}; - rclcpp::Service::SharedPtr - set_sensor_mounting_service_server_{}; - rclcpp::Service::SharedPtr - set_vehicle_parameters_service_server_{}; - rclcpp::Service::SharedPtr - set_radar_parameters_service_server_{}; + NEBULA_SERVICE_PTR(continental_srvs::srv::ContinentalArs548SetNetworkConfiguration) + set_network_configuration_service_server_{}; + NEBULA_SERVICE_PTR(continental_srvs::srv::ContinentalArs548SetSensorMounting) + set_sensor_mounting_service_server_{}; + NEBULA_SERVICE_PTR(continental_srvs::srv::ContinentalArs548SetVehicleParameters) + set_vehicle_parameters_service_server_{}; + NEBULA_SERVICE_PTR(continental_srvs::srv::ContinentalArs548SetRadarParameters) + set_radar_parameters_service_server_{}; }; } // namespace nebula::ros diff --git a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_ros_wrapper.hpp b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_ros_wrapper.hpp index bcebacade..6653dcc7a 100644 --- a/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_ros_wrapper.hpp +++ b/src/nebula_continental/nebula_continental/include/nebula_continental/continental_ars548_ros_wrapper.hpp @@ -16,6 +16,7 @@ #include "nebula_continental/continental_ars548_decoder_wrapper.hpp" #include "nebula_continental/continental_ars548_hw_interface_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/parameter_descriptors.hpp" #include @@ -39,7 +40,7 @@ namespace nebula::ros { /// @brief Ros wrapper of continental ars548 driver -class ContinentalARS548RosWrapper final : public rclcpp::Node +class ContinentalARS548RosWrapper final : public nebula::agnocast_wrapper::Node { public: explicit ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options); @@ -58,7 +59,8 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node void receive_packet_callback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void receive_packets_callback(std::unique_ptr packets_msg_ptr); + void receive_packets_callback(NEBULA_MESSAGE_CONST_SHARED_PTR(nebula_msgs::msg::NebulaPackets) + packets_msg_ptr); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status @@ -79,7 +81,7 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node std::shared_ptr config_ptr_{}; - rclcpp::Subscription::SharedPtr packets_sub_{}; + NEBULA_SUBSCRIPTION_PTR(nebula_msgs::msg::NebulaPackets) packets_sub_ {}; bool launch_hw_{}; @@ -88,7 +90,7 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_{}; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_{}; }; } // namespace nebula::ros diff --git a/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp b/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp index f7efc60b5..fd4954211 100644 --- a/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp +++ b/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp @@ -36,7 +36,7 @@ namespace nebula::ros using std::chrono_literals::operator""ms; ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr & config_ptr, bool launch_hw) @@ -147,7 +147,8 @@ Status ContinentalARS548DecoderWrapper::initialize_driver( return Status::OK; } -void ContinentalARS548DecoderWrapper::initialize_sync_diagnostics(rclcpp::Node * const parent_node) +void ContinentalARS548DecoderWrapper::initialize_sync_diagnostics( + nebula::agnocast_wrapper::Node * const parent_node) { std::scoped_lock lock(mtx_config_ptr_, mtx_driver_ptr_); if (!config_ptr_->sync_diagnostics_topic) { @@ -204,7 +205,7 @@ void ContinentalARS548DecoderWrapper::detection_list_callback( detection_pointcloud_pub_->get_subscription_count() > 0 || detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { const auto detection_pointcloud = convert_to_pointcloud(*msg); - auto detection_pointcloud_msg_ptr = std::make_unique(); + auto detection_pointcloud_msg_ptr = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(detection_pointcloud_pub_); *detection_pointcloud_msg_ptr = nebula::ros::to_ros_msg(detection_pointcloud); detection_pointcloud_msg_ptr->header = msg->header; @@ -222,7 +223,9 @@ void ContinentalARS548DecoderWrapper::detection_list_callback( if ( detection_list_pub_->get_subscription_count() > 0 || detection_list_pub_->get_intra_process_subscription_count() > 0) { - detection_list_pub_->publish(std::move(msg)); + auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(detection_list_pub_); + *out = *msg; + detection_list_pub_->publish(std::move(out)); } std::shared_lock lock_cfg(mtx_config_ptr_); @@ -245,7 +248,7 @@ void ContinentalARS548DecoderWrapper::object_list_callback( object_pointcloud_pub_->get_subscription_count() > 0 || object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { const auto object_pointcloud = convert_to_pointcloud(*msg); - auto object_pointcloud_msg_ptr = std::make_unique(); + auto object_pointcloud_msg_ptr = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(object_pointcloud_pub_); *object_pointcloud_msg_ptr = nebula::ros::to_ros_msg(object_pointcloud); object_pointcloud_msg_ptr->header = msg->header; @@ -277,7 +280,9 @@ void ContinentalARS548DecoderWrapper::object_list_callback( if ( object_list_pub_->get_subscription_count() > 0 || object_list_pub_->get_intra_process_subscription_count() > 0) { - object_list_pub_->publish(std::move(msg)); + auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(object_list_pub_); + *out = std::move(*msg); + object_list_pub_->publish(std::move(out)); } objects_rate_bound_status_.tick(); } @@ -448,7 +453,9 @@ void ContinentalARS548DecoderWrapper::packets_callback( if ( packets_pub_ && (packets_pub_->get_subscription_count() > 0 || packets_pub_->get_intra_process_subscription_count() > 0)) { - packets_pub_->publish(std::move(msg)); + auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(packets_pub_); + *out = std::move(*msg); + packets_pub_->publish(std::move(out)); } } diff --git a/src/nebula_continental/nebula_continental/src/continental_ars548_hw_interface_wrapper.cpp b/src/nebula_continental/nebula_continental/src/continental_ars548_hw_interface_wrapper.cpp index b065357da..8d094ab8d 100644 --- a/src/nebula_continental/nebula_continental/src/continental_ars548_hw_interface_wrapper.cpp +++ b/src/nebula_continental/nebula_continental/src/continental_ars548_hw_interface_wrapper.cpp @@ -27,7 +27,7 @@ namespace nebula::ros { ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr & config_ptr) : parent_node_(parent_node), @@ -136,7 +136,7 @@ ContinentalARS548HwInterfaceWrapper::hw_interface() const } void ContinentalARS548HwInterfaceWrapper::odometry_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) + NEBULA_MESSAGE_CONST_SHARED_PTR(geometry_msgs::msg::TwistWithCovarianceStamped) msg) { using nebula::drivers::continental_ars548::max_odometry_hz; using nebula::drivers::continental_ars548::min_odometry_hz; @@ -179,7 +179,7 @@ void ContinentalARS548HwInterfaceWrapper::odometry_callback( } void ContinentalARS548HwInterfaceWrapper::acceleration_callback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) + NEBULA_MESSAGE_CONST_SHARED_PTR(geometry_msgs::msg::AccelWithCovarianceStamped) msg) { using nebula::drivers::continental_ars548::max_odometry_hz; using nebula::drivers::continental_ars548::min_odometry_hz; @@ -204,7 +204,7 @@ void ContinentalARS548HwInterfaceWrapper::acceleration_callback( } void ContinentalARS548HwInterfaceWrapper::steering_angle_callback( - const std_msgs::msg::Float32::SharedPtr msg) + NEBULA_MESSAGE_CONST_SHARED_PTR(std_msgs::msg::Float32) msg) { using nebula::drivers::continental_ars548::max_odometry_hz; using nebula::drivers::continental_ars548::min_odometry_hz; diff --git a/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp b/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp index b716d7617..d946697b8 100644 --- a/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp +++ b/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp @@ -29,7 +29,7 @@ namespace nebula::ros { ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node( +: nebula::agnocast_wrapper::Node( "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED) { @@ -64,8 +64,7 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio std::bind( &ContinentalARS548RosWrapper::receive_packets_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( - get_logger(), - "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + get_logger(), "Hardware connection disabled, listening for packets on " << "nebula_packets"); } // Register parameter callback after all params have been declared. Otherwise it would be called @@ -161,7 +160,7 @@ Status ContinentalARS548RosWrapper::validate_and_set_config( } void ContinentalARS548RosWrapper::receive_packets_callback( - std::unique_ptr packets_msg_ptr) + NEBULA_MESSAGE_CONST_SHARED_PTR(nebula_msgs::msg::NebulaPackets) packets_msg_ptr) { if (hw_interface_wrapper_) { RCLCPP_ERROR_THROTTLE( @@ -171,10 +170,10 @@ void ContinentalARS548RosWrapper::receive_packets_callback( return; } - for (auto & packet : packets_msg_ptr->packets) { + for (const auto & packet : packets_msg_ptr->packets) { auto nebula_packet_ptr = std::make_unique(); nebula_packet_ptr->stamp = packet.stamp; - nebula_packet_ptr->data = std::move(packet.data); + nebula_packet_ptr->data = packet.data; decoder_wrapper_->process_packet(std::move(nebula_packet_ptr)); } From dba543a13a66592125841c41fe8578d64acbd9ab Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Mon, 27 Jul 2026 13:29:42 +0900 Subject: [PATCH 3/9] fix(continental_ars548_ros_wrapper): publish pass-through messages via publish(const &) The ALLOCATE_OUTPUT + *out = std::move(*msg) pattern stole msg's buffer, which was allocated on the normal heap (outside the borrow window), so the payload never entered shared memory and agnocast zero-copy delivered an invalid pointer to cross-process subscribers. publish(const &) borrows inside the window and copies into shared memory, which is correct on both the rclcpp and agnocast backends. Signed-off-by: Koichi Imai --- .../src/continental_ars548_decoder_wrapper.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp b/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp index fd4954211..cfe9ad4f5 100644 --- a/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp +++ b/src/nebula_continental/nebula_continental/src/continental_ars548_decoder_wrapper.cpp @@ -223,9 +223,7 @@ void ContinentalARS548DecoderWrapper::detection_list_callback( if ( detection_list_pub_->get_subscription_count() > 0 || detection_list_pub_->get_intra_process_subscription_count() > 0) { - auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(detection_list_pub_); - *out = *msg; - detection_list_pub_->publish(std::move(out)); + detection_list_pub_->publish(*msg); } std::shared_lock lock_cfg(mtx_config_ptr_); @@ -280,9 +278,7 @@ void ContinentalARS548DecoderWrapper::object_list_callback( if ( object_list_pub_->get_subscription_count() > 0 || object_list_pub_->get_intra_process_subscription_count() > 0) { - auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(object_list_pub_); - *out = std::move(*msg); - object_list_pub_->publish(std::move(out)); + object_list_pub_->publish(*msg); } objects_rate_bound_status_.tick(); } @@ -453,9 +449,7 @@ void ContinentalARS548DecoderWrapper::packets_callback( if ( packets_pub_ && (packets_pub_->get_subscription_count() > 0 || packets_pub_->get_intra_process_subscription_count() > 0)) { - auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(packets_pub_); - *out = std::move(*msg); - packets_pub_->publish(std::move(out)); + packets_pub_->publish(*msg); } } From b938e2d43ca3e43cd56bb54bb0fb6525aad56561 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Mon, 27 Jul 2026 15:05:18 +0900 Subject: [PATCH 4/9] fix(nebula_core_ros): include renamed nebula_agnocast_wrapper.hpp in sync_tooling_worker Signed-off-by: Koichi Imai --- .../nebula_core_ros/sync_tooling/sync_tooling_worker.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp index e6ccde345..860f44cc2 100644 --- a/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp +++ b/src/nebula_core/nebula_core_ros/include/nebula_core_ros/sync_tooling/sync_tooling_worker.hpp @@ -14,7 +14,7 @@ #pragma once -#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" #include "nebula_core_ros/agnocast_wrapper/node.hpp" #include From 73dec4529649ceace3e81f29f2bb1b877c12c839 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Thu, 23 Jul 2026 13:49:43 +0900 Subject: [PATCH 5/9] feat(nebula_hesai): migrate HesaiRosWrapper to agnocast_wrapper::Node (Method 2) Signed-off-by: Koichi Imai --- src/nebula_hesai/nebula_hesai/CMakeLists.txt | 13 +------ .../include/nebula_hesai/decoder_wrapper.hpp | 22 +++++++----- .../functional_safety_diagnostic_task.hpp | 3 +- .../nebula_hesai/hesai_ros_wrapper.hpp | 16 +++++---- .../nebula_hesai/hw_interface_wrapper.hpp | 4 ++- .../nebula_hesai/hw_monitor_wrapper.hpp | 12 ++++--- .../nebula_hesai/src/decoder_wrapper.cpp | 34 +++++++++---------- .../nebula_hesai/src/hesai_ros_wrapper.cpp | 10 +++--- .../nebula_hesai/src/hw_interface_wrapper.cpp | 6 ++-- .../nebula_hesai/src/hw_monitor_wrapper.cpp | 5 +-- 10 files changed, 63 insertions(+), 62 deletions(-) diff --git a/src/nebula_hesai/nebula_hesai/CMakeLists.txt b/src/nebula_hesai/nebula_hesai/CMakeLists.txt index 710ee5d51..6f615b311 100644 --- a/src/nebula_hesai/nebula_hesai/CMakeLists.txt +++ b/src/nebula_hesai/nebula_hesai/CMakeLists.txt @@ -11,11 +11,6 @@ endif() if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocast_components_FOUND) message(FATAL_ERROR "agnocast_components is required when Agnocast is enabled") endif() -if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocast_cie_thread_configurator_FOUND) - message( - FATAL_ERROR - "agnocast_cie_thread_configurator is required when Agnocast is enabled") -endif() # Hesai add_library( @@ -51,12 +46,6 @@ ament_target_dependencies( if("$ENV{ENABLE_AGNOCAST}") message(STATUS "Agnocast support is enabled") ament_target_dependencies(hesai_ros_wrapper PUBLIC agnocastlib) - # Used only by nebula_core_ros/cie_thread_factory.hpp, whose Agnocast branch - # is compiled into this package's translation units; not part of the exported - # interface. - target_link_libraries( - hesai_ros_wrapper - PRIVATE agnocast_cie_thread_configurator::agnocast_thread_configurator) add_definitions(-DUSE_AGNOCAST_ENABLED) endif() @@ -64,7 +53,7 @@ if("$ENV{ENABLE_AGNOCAST}") agnocast_components_register_node( hesai_ros_wrapper PLUGIN "HesaiRosWrapper" EXECUTABLE hesai_ros_wrapper_node - EXECUTOR CallbackIsolatedAgnocastExecutor) + EXECUTOR AgnocastOnlyCallbackIsolatedExecutor) else() rclcpp_components_register_node(hesai_ros_wrapper PLUGIN "HesaiRosWrapper" EXECUTABLE hesai_ros_wrapper_node) diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp index 6de3faf80..ad97b1355 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp @@ -14,7 +14,9 @@ #pragma once -#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/diagnostic_updater.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/diagnostics/rate_bound_status.hpp" #include "nebula_core_ros/single_consumer_processor.hpp" #include "nebula_hesai/diagnostics/functional_safety_diagnostic_task.hpp" @@ -47,10 +49,11 @@ class HesaiDecoderWrapper { public: HesaiDecoderWrapper( - rclcpp::Node * parent_node, + nebula::agnocast_wrapper::Node * parent_node, const std::shared_ptr & config, const std::shared_ptr & calibration, - diagnostic_updater::Updater & diagnostic_updater, bool publish_packets); + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, + bool publish_packets); /// @brief Process a cloud packet and return metadata /// @param packet_msg The packet to process @@ -91,7 +94,7 @@ class HesaiDecoderWrapper } static custom_diagnostic_tasks::RateBoundStatus make_rate_bound_status( - uint16_t rpm, rclcpp::Node & node) + uint16_t rpm, nebula::agnocast_wrapper::Node & node) { double nominal_rate_hz = drivers::rpm2hz(rpm); @@ -124,10 +127,11 @@ class HesaiDecoderWrapper } void initialize_functional_safety( - diagnostic_updater::Updater & diagnostic_updater, + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, const std::optional & fs_config); - void initialize_packet_loss_diagnostic(diagnostic_updater::Updater & diagnostic_updater); + void initialize_packet_loss_diagnostic( + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater); std::pair< std::shared_ptr, @@ -140,7 +144,7 @@ class HesaiDecoderWrapper nebula::Status status_; rclcpp::Logger logger_; - rclcpp::Node & parent_node_; + nebula::agnocast_wrapper::Node & parent_node_; std::shared_ptr sensor_cfg_; std::shared_ptr calibration_cfg_ptr_; @@ -149,7 +153,7 @@ class HesaiDecoderWrapper std::mutex mtx_driver_ptr_; pandar_msgs::msg::PandarScan::UniquePtr current_scan_msg_; - rclcpp::Publisher::SharedPtr packets_pub_; + NEBULA_PUBLISHER_PTR(pandar_msgs::msg::PandarScan) packets_pub_; std::optional> packets_pub_thread_; @@ -163,7 +167,7 @@ class HesaiDecoderWrapper std::optional functional_safety_diagnostic_; std::optional packet_loss_diagnostic_; - autoware_utils_debug::DebugPublisher debug_publisher_; + autoware_utils_debug::BasicDebugPublisher debug_publisher_; struct PerformanceCounters { diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/diagnostics/functional_safety_diagnostic_task.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/diagnostics/functional_safety_diagnostic_task.hpp index f8cbd4e63..a7b42eb5c 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/diagnostics/functional_safety_diagnostic_task.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/diagnostics/functional_safety_diagnostic_task.hpp @@ -14,6 +14,7 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/diagnostics/liveness_monitor.hpp" #include "nebula_core_ros/diagnostics/severity_latch.hpp" @@ -108,7 +109,7 @@ class FunctionalSafetyDiagnosticTask : public diagnostic_updater::CompositeDiagn { public: explicit FunctionalSafetyDiagnosticTask( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::unique_ptr status_processor) : CompositeDiagnosticTask("Functional safety status"), liveness_monitor_("Liveness", parent_node, 100ms), diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp index fc13208a4..eee7c051d 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hesai_ros_wrapper.hpp @@ -17,13 +17,14 @@ #include "nebula_core_common/nebula_common.hpp" #include "nebula_core_common/nebula_status.hpp" #include "nebula_core_hw_interfaces/connections/udp.hpp" +#include "nebula_core_ros/agnocast_wrapper/diagnostic_updater.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/sync_tooling/sync_tooling_worker.hpp" #include "nebula_hesai/decoder_wrapper.hpp" #include "nebula_hesai/hw_interface_wrapper.hpp" #include "nebula_hesai/hw_monitor_wrapper.hpp" #include "nebula_hesai_common/hesai_common.hpp" -#include #include #include @@ -44,7 +45,7 @@ namespace nebula::ros { /// @brief Ros wrapper of hesai driver -class HesaiRosWrapper final : public rclcpp::Node +class HesaiRosWrapper final : public nebula::agnocast_wrapper::Node { using get_calibration_result_t = nebula::util::expected< std::shared_ptr, nebula::Status>; @@ -87,7 +88,8 @@ class HesaiRosWrapper final : public rclcpp::Node const std::vector & packet, const drivers::connections::UdpSocket::RxMetadata & metadata); - void receive_scan_message_callback(std::unique_ptr scan_msg); + void receive_scan_message_callback(NEBULA_MESSAGE_CONST_SHARED_PTR(pandar_msgs::msg::PandarScan) + scan_msg); Status declare_and_get_sensor_config_params(); @@ -123,7 +125,7 @@ class HesaiRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; - rclcpp::Subscription::SharedPtr packets_sub_{}; + NEBULA_SUBSCRIPTION_PTR(pandar_msgs::msg::PandarScan) packets_sub_ {}; bool launch_hw_; /// @brief Only exists if launch_hw_ is false. Tracks replay state, e.g. to reset state when a @@ -137,13 +139,13 @@ class HesaiRosWrapper final : public rclcpp::Node std::optional decoder_wrapper_; /// @brief Diagnostics that are not time or safety-critical - diagnostic_updater::Updater diagnostic_updater_general_; + nebula::agnocast_wrapper::diagnostic_updater::Updater diagnostic_updater_general_; /// @brief Diagnostics that are related to functional safety - diagnostic_updater::Updater diagnostic_updater_functional_safety_; + nebula::agnocast_wrapper::diagnostic_updater::Updater diagnostic_updater_functional_safety_; std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_; }; } // namespace nebula::ros diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp index 1d7702f03..cfec22a55 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp @@ -14,6 +14,8 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/node.hpp" + #include "nebula_hesai_hw_interfaces/hesai_cmd_response.hpp" #include @@ -28,7 +30,7 @@ class HesaiHwInterfaceWrapper { public: HesaiHwInterfaceWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr & config, bool use_udp_only = false); diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_monitor_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_monitor_wrapper.hpp index c9a864781..7e3d91b2f 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_monitor_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_monitor_wrapper.hpp @@ -14,6 +14,8 @@ #pragma once +#include "nebula_core_ros/agnocast_wrapper/diagnostic_updater.hpp" +#include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/single_consumer_processor.hpp" #include "nebula_core_ros/sync_tooling/sync_tooling_worker.hpp" @@ -41,7 +43,8 @@ class HesaiHwMonitorWrapper { public: HesaiHwMonitorWrapper( - rclcpp::Node * parent_node, diagnostic_updater::Updater & diagnostic_updater, + nebula::agnocast_wrapper::Node * parent_node, + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, const std::shared_ptr & hw_interface, const std::shared_ptr & config, const std::shared_ptr & sync_tooling_worker); @@ -59,7 +62,8 @@ class HesaiHwMonitorWrapper diagnostic_updater::DiagnosticStatusWrapper & diagnostics, const std::string & key, const json & value); - void initialize_hesai_diagnostics(diagnostic_updater::Updater & diagnostic_updater); + void initialize_hesai_diagnostics( + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater); static std::string get_ptree_value(boost::property_tree::ptree * pt, const std::string & key); @@ -95,12 +99,12 @@ class HesaiHwMonitorWrapper nebula::Status status_; const std::shared_ptr hw_interface_; - rclcpp::Node * const parent_node_; + nebula::agnocast_wrapper::Node * const parent_node_; uint16_t diag_span_ms_; bool monitor_enabled_; - rclcpp::TimerBase::SharedPtr fetch_diagnostics_timer_; + NEBULA_TIMER_PTR fetch_diagnostics_timer_; std::shared_ptr current_status_; std::shared_ptr current_monitor_; diff --git a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp index 2391456d8..0f9d2013d 100644 --- a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp @@ -2,8 +2,7 @@ #include "nebula_hesai/decoder_wrapper.hpp" -#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" -#include "nebula_core_ros/cie_thread_factory.hpp" +#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" #include "nebula_core_ros/point_cloud_conversions.hpp" #include "nebula_core_ros/rclcpp_logger.hpp" #include "nebula_hesai/diagnostics/functional_safety_advanced.hpp" @@ -35,10 +34,10 @@ namespace nebula::ros using namespace std::chrono_literals; // NOLINT(build/namespaces) HesaiDecoderWrapper::HesaiDecoderWrapper( - rclcpp::Node * parent_node, + nebula::agnocast_wrapper::Node * parent_node, const std::shared_ptr & config, const std::shared_ptr & calibration, - diagnostic_updater::Updater & diagnostic_updater, bool publish_packets) + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, bool publish_packets) : status_(nebula::Status::NOT_INITIALIZED), logger_(parent_node->get_logger().get_child("HesaiDecoder")), parent_node_(*parent_node), @@ -75,26 +74,27 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( current_scan_msg_ = std::make_unique(); packets_pub_ = parent_node->create_publisher( "pandar_packets", rclcpp::SensorDataQoS()); - packets_pub_thread_.emplace( [this](pandar_msgs::msg::PandarScan::UniquePtr && msg) { if (packets_pub_) { - packets_pub_->publish(std::move(msg)); + auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(packets_pub_); + *out = std::move(*msg); + packets_pub_->publish(std::move(out)); } }, - 10, make_cie_thread_factory("nebula_hesai_packets_pub@" + sensor_cfg_->frame_id)); + 10); } auto qos_profile = rmw_qos_profile_sensor_data; auto pointcloud_qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); - nebula_points_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( - sensor_msgs::msg::PointCloud2, &parent_node_, "pandar_points", pointcloud_qos); - aw_points_base_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( - sensor_msgs::msg::PointCloud2, &parent_node_, "aw_points", pointcloud_qos); - aw_points_ex_pub_ = NEBULA_CREATE_PUBLISHER2_ON_NODE( - sensor_msgs::msg::PointCloud2, &parent_node_, "aw_points_ex", pointcloud_qos); + nebula_points_pub_ = + parent_node_.create_publisher("pandar_points", pointcloud_qos); + aw_points_base_pub_ = + parent_node_.create_publisher("aw_points", pointcloud_qos); + aw_points_ex_pub_ = + parent_node_.create_publisher("aw_points_ex", pointcloud_qos); RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); @@ -246,7 +246,7 @@ void HesaiDecoderWrapper::publish_cloud( } void HesaiDecoderWrapper::initialize_functional_safety( - diagnostic_updater::Updater & diagnostic_updater, + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, const std::optional & fs_config) { if (!drivers::supports_functional_safety(sensor_cfg_->sensor_model)) { @@ -268,7 +268,7 @@ void HesaiDecoderWrapper::initialize_functional_safety( } void HesaiDecoderWrapper::initialize_packet_loss_diagnostic( - diagnostic_updater::Updater & diagnostic_updater) + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater) { if (!drivers::supports_packet_loss_detection(sensor_cfg_->sensor_model)) { return; @@ -291,8 +291,8 @@ HesaiDecoderWrapper::initialize_blockage_mask_plugin() auto blockage_mask_plugin = std::make_shared( sensor_cfg_->blockage_mask_horizontal_bin_size_mdeg.value()); - auto blockage_mask_pub = NEBULA_CREATE_PUBLISHER2_ON_NODE( - sensor_msgs::msg::Image, &parent_node_, "blockage_mask", rclcpp::SensorDataQoS()); + auto blockage_mask_pub = parent_node_.create_publisher( + "blockage_mask", rclcpp::SensorDataQoS()); blockage_mask_plugin->set_callback( [this, blockage_mask_pub]( diff --git a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp index 5f92f2407..f07253896 100644 --- a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp @@ -31,7 +31,8 @@ namespace nebula::ros { HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), +: nebula::agnocast_wrapper::Node( + "hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), wrapper_status_(Status::NOT_INITIALIZED), sensor_cfg_ptr_(nullptr), diagnostic_updater_general_((declare_parameter("diagnostic_updater.use_fqn", true), this)), @@ -114,8 +115,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) "pandar_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::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()); + get_logger(), "Hardware connection disabled, listening for packets on " << "pandar_packets"); } // Register parameter callback after all params have been declared. Otherwise it would be called @@ -406,7 +406,7 @@ Status HesaiRosWrapper::validate_and_set_config( } void HesaiRosWrapper::receive_scan_message_callback( - std::unique_ptr scan_msg) + NEBULA_MESSAGE_CONST_SHARED_PTR(pandar_msgs::msg::PandarScan) scan_msg) { util::Stopwatch receive_watch; if (hw_interface_wrapper_) { @@ -424,7 +424,7 @@ void HesaiRosWrapper::receive_scan_message_callback( replay_state_ = ReplayState{scan_msg->packets.front().stamp}; } - for (auto & pkt : scan_msg->packets) { + for (const auto & pkt : scan_msg->packets) { auto nebula_pkt_ptr = std::make_unique(); nebula_pkt_ptr->stamp = pkt.stamp; std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); diff --git a/src/nebula_hesai/nebula_hesai/src/hw_interface_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hw_interface_wrapper.cpp index a2a9da3fe..3b6fb0495 100644 --- a/src/nebula_hesai/nebula_hesai/src/hw_interface_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hw_interface_wrapper.cpp @@ -2,7 +2,6 @@ #include "nebula_hesai/hw_interface_wrapper.hpp" -#include "nebula_core_ros/cie_thread_factory.hpp" #include "nebula_core_ros/parameter_descriptors.hpp" #include "nebula_core_ros/rclcpp_logger.hpp" #include "nebula_hesai_hw_interfaces/hesai_cmd_response.hpp" @@ -16,12 +15,11 @@ namespace nebula::ros { HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( - rclcpp::Node * const parent_node, + nebula::agnocast_wrapper::Node * const parent_node, std::shared_ptr & config, bool use_udp_only) : hw_interface_( std::make_shared( - drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"), - make_cie_thread_factory("nebula_hesai_udp_receiver@" + config->frame_id))), + drivers::loggers::RclcppLogger(parent_node->get_logger()).child("HwInterface"))), logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(Status::NOT_INITIALIZED), use_udp_only_(use_udp_only) diff --git a/src/nebula_hesai/nebula_hesai/src/hw_monitor_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hw_monitor_wrapper.cpp index d1d14d826..5dcaa557a 100644 --- a/src/nebula_hesai/nebula_hesai/src/hw_monitor_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hw_monitor_wrapper.cpp @@ -47,7 +47,8 @@ void HesaiHwMonitorWrapper::add_json_item_to_diagnostics( } HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( - rclcpp::Node * const parent_node, diagnostic_updater::Updater & diagnostic_updater, + nebula::agnocast_wrapper::Node * const parent_node, + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater, const std::shared_ptr & hw_interface, const std::shared_ptr & config, const std::shared_ptr & sync_tooling_worker) @@ -64,7 +65,7 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( } void HesaiHwMonitorWrapper::initialize_hesai_diagnostics( - diagnostic_updater::Updater & diagnostic_updater) + nebula::agnocast_wrapper::diagnostic_updater::Updater & diagnostic_updater) { using std::chrono_literals::operator""s; std::ostringstream os; From 50d03927c6d432e95a90479217dc84fc2bdd89f8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Jul 2026 04:53:10 +0000 Subject: [PATCH 6/9] ci(pre-commit): autofix --- .../nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp index cfec22a55..bd0ce02ee 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/hw_interface_wrapper.hpp @@ -15,7 +15,6 @@ #pragma once #include "nebula_core_ros/agnocast_wrapper/node.hpp" - #include "nebula_hesai_hw_interfaces/hesai_cmd_response.hpp" #include From 7dfb633222decb075d9b2c0f34b34d8902dd2f83 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Mon, 27 Jul 2026 15:13:54 +0900 Subject: [PATCH 7/9] fix(nebula_hesai): renamed wrapper include, const-ref packets publish, restore cie thread factory - include the post-#472 nebula_agnocast_wrapper.hpp (old autoware_agnocast_wrapper.hpp no longer exists) - publish raw packets via publish(*msg) so the payload is materialized into shared memory (a plain std::move would leave it on the normal heap, invalid for cross-process subscribers) - restore make_cie_thread_factory for the packets publish thread; the migration had dropped it, which would leave that non-ROS thread unmanaged by the Agnocast CIE thread configurator and lose its configured priority/affinity Signed-off-by: Koichi Imai --- src/nebula_hesai/nebula_hesai/CMakeLists.txt | 11 +++++++++++ .../include/nebula_hesai/decoder_wrapper.hpp | 2 +- src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp | 9 ++++----- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/nebula_hesai/nebula_hesai/CMakeLists.txt b/src/nebula_hesai/nebula_hesai/CMakeLists.txt index 6f615b311..43df411e2 100644 --- a/src/nebula_hesai/nebula_hesai/CMakeLists.txt +++ b/src/nebula_hesai/nebula_hesai/CMakeLists.txt @@ -11,6 +11,11 @@ endif() if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocast_components_FOUND) message(FATAL_ERROR "agnocast_components is required when Agnocast is enabled") endif() +if("$ENV{ENABLE_AGNOCAST}" AND NOT agnocast_cie_thread_configurator_FOUND) + message( + FATAL_ERROR + "agnocast_cie_thread_configurator is required when Agnocast is enabled") +endif() # Hesai add_library( @@ -47,6 +52,12 @@ if("$ENV{ENABLE_AGNOCAST}") message(STATUS "Agnocast support is enabled") ament_target_dependencies(hesai_ros_wrapper PUBLIC agnocastlib) add_definitions(-DUSE_AGNOCAST_ENABLED) + # Used only by nebula_core_ros/cie_thread_factory.hpp, whose Agnocast branch + # is compiled into this package's translation units; not part of the exported + # interface. + target_link_libraries( + hesai_ros_wrapper + PRIVATE agnocast_cie_thread_configurator::agnocast_thread_configurator) endif() if("$ENV{ENABLE_AGNOCAST}") diff --git a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp index ad97b1355..aea374f1b 100644 --- a/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp +++ b/src/nebula_hesai/nebula_hesai/include/nebula_hesai/decoder_wrapper.hpp @@ -14,8 +14,8 @@ #pragma once -#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" #include "nebula_core_ros/agnocast_wrapper/diagnostic_updater.hpp" +#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" #include "nebula_core_ros/agnocast_wrapper/node.hpp" #include "nebula_core_ros/diagnostics/rate_bound_status.hpp" #include "nebula_core_ros/single_consumer_processor.hpp" diff --git a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp index 0f9d2013d..30e19aa9e 100644 --- a/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/decoder_wrapper.cpp @@ -2,7 +2,8 @@ #include "nebula_hesai/decoder_wrapper.hpp" -#include "nebula_core_ros/agnocast_wrapper/autoware_agnocast_wrapper.hpp" +#include "nebula_core_ros/agnocast_wrapper/nebula_agnocast_wrapper.hpp" +#include "nebula_core_ros/cie_thread_factory.hpp" #include "nebula_core_ros/point_cloud_conversions.hpp" #include "nebula_core_ros/rclcpp_logger.hpp" #include "nebula_hesai/diagnostics/functional_safety_advanced.hpp" @@ -77,12 +78,10 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( packets_pub_thread_.emplace( [this](pandar_msgs::msg::PandarScan::UniquePtr && msg) { if (packets_pub_) { - auto out = ALLOCATE_OUTPUT_MESSAGE_UNIQUE(packets_pub_); - *out = std::move(*msg); - packets_pub_->publish(std::move(out)); + packets_pub_->publish(*msg); } }, - 10); + 10, make_cie_thread_factory("nebula_hesai_packets_pub@" + sensor_cfg_->frame_id)); } auto qos_profile = rmw_qos_profile_sensor_data; From 0a0a6e6819304abbb69c0de04c4fbac6ff0eee63 Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Wed, 29 Jul 2026 14:27:28 +0900 Subject: [PATCH 8/9] fix(continental_ars548_ros_wrapper): log the resolved packets topic again The migration replaced `packets_sub_->get_topic_name()` with the relative topic literal, which hides the actual (namespaced/remapped) topic and makes troubleshooting harder. The wrapper's Subscription has no get_topic_name(), so resolve the name through the node's topics interface instead; this yields the same fully-qualified name as before on both backends. Signed-off-by: Koichi Imai --- .../src/continental_ars548_ros_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp b/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp index d946697b8..c173e35b1 100644 --- a/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp +++ b/src/nebula_continental/nebula_continental/src/continental_ars548_ros_wrapper.cpp @@ -59,12 +59,14 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio &ContinentalARS548RosWrapper::receive_packet_callback, this, std::placeholders::_1)); stream_start(); } else { + const std::string packets_topic = "nebula_packets"; packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), + packets_topic, rclcpp::SensorDataQoS(), std::bind( &ContinentalARS548RosWrapper::receive_packets_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( - get_logger(), "Hardware connection disabled, listening for packets on " << "nebula_packets"); + get_logger(), "Hardware connection disabled, listening for packets on " + << get_node_topics_interface()->resolve_topic_name(packets_topic)); } // Register parameter callback after all params have been declared. Otherwise it would be called From f2e4653d1494490471ad1d9e1b8afe8673c2b99d Mon Sep 17 00:00:00 2001 From: Koichi Imai Date: Wed, 29 Jul 2026 14:27:48 +0900 Subject: [PATCH 9/9] fix(nebula_hesai): log the resolved packets topic again The migration replaced `packets_sub_->get_topic_name()` with the relative topic literal, which hides the actual (namespaced/remapped) topic and makes troubleshooting harder. The wrapper's Subscription has no get_topic_name(), so resolve the name through the node's topics interface instead; this yields the same fully-qualified name as before on both backends. Signed-off-by: Koichi Imai --- src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp index f07253896..921df5009 100644 --- a/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp +++ b/src/nebula_hesai/nebula_hesai/src/hesai_ros_wrapper.cpp @@ -111,11 +111,13 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) std::placeholders::_2)); stream_start(); } else { + const std::string packets_topic = "pandar_packets"; packets_sub_ = create_subscription( - "pandar_packets", rclcpp::SensorDataQoS(), + packets_topic, rclcpp::SensorDataQoS(), std::bind(&HesaiRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( - get_logger(), "Hardware connection disabled, listening for packets on " << "pandar_packets"); + get_logger(), "Hardware connection disabled, listening for packets on " + << get_node_topics_interface()->resolve_topic_name(packets_topic)); } // Register parameter callback after all params have been declared. Otherwise it would be called