feat(nebula_core_ros): make SyncToolingWorker compatible with agnocast_wrapper::Node

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"

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<const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>
     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<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub_{};
-
-  rclcpp::Publisher<continental_msgs::msg::ContinentalArs548DetectionList>::SharedPtr
-    detection_list_pub_{};
-  rclcpp::Publisher<continental_msgs::msg::ContinentalArs548ObjectList>::SharedPtr
-    object_list_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr object_pointcloud_pub_{};
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr detection_pointcloud_pub_{};
-  rclcpp::Publisher<autoware_sensing_msgs::msg::RadarObjects>::SharedPtr autoware_objects_pub_{};
-  rclcpp::Publisher<radar_msgs::msg::RadarScan>::SharedPtr scan_raw_pub_{};
-  rclcpp::Publisher<radar_msgs::msg::RadarTracks>::SharedPtr objects_raw_pub_{};
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr objects_markers_pub_{};
-  rclcpp::Publisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_{};
-  rclcpp::Publisher<autoware_sensing_msgs::msg::RadarInfo>::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};

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 <nebula_continental_common/continental_ars548.hpp>
@@ -37,7 +38,7 @@ class ContinentalARS548HwInterfaceWrapper
 {
 public:
   ContinentalARS548HwInterfaceWrapper(
-    rclcpp::Node * const parent_node,
+    nebula::agnocast_wrapper::Node * const parent_node,
     std::shared_ptr<const drivers::continental_ars548::ContinentalARS548SensorConfiguration> &
       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,31 +106,30 @@ class ContinentalARS548HwInterfaceWrapper
     const std::shared_ptr<continental_srvs::srv::ContinentalArs548SetRadarParameters::Response>
       response);
 
-  rclcpp::Node * const parent_node_;
+  nebula::agnocast_wrapper::Node * const parent_node_;
   std::shared_ptr<drivers::continental_ars548::ContinentalARS548HwInterface> hw_interface_{};
   rclcpp::Logger logger_;
   nebula::Status status_{};
   std::shared_ptr<const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>
     config_ptr_{};
 
-  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr odometry_sub_{};
-  rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr
-    acceleration_sub_{};
-  rclcpp::Subscription<std_msgs::msg::Float32>::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_;
   nebula::util::RateChecker steering_angle_rate_checker_;
 
   bool standstill_{true};
 
-  rclcpp::Service<continental_srvs::srv::ContinentalArs548SetNetworkConfiguration>::SharedPtr
-    set_network_configuration_service_server_{};
-  rclcpp::Service<continental_srvs::srv::ContinentalArs548SetSensorMounting>::SharedPtr
-    set_sensor_mounting_service_server_{};
-  rclcpp::Service<continental_srvs::srv::ContinentalArs548SetVehicleParameters>::SharedPtr
-    set_vehicle_parameters_service_server_{};
-  rclcpp::Service<continental_srvs::srv::ContinentalArs548SetRadarParameters>::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

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 <nebula_continental_common/continental_ars548.hpp>
@@ -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<nebula_msgs::msg::NebulaPacket> packet_msg_ptr);
 
   /// @brief Callback from replayed NebulaPackets
-  void receive_packets_callback(std::unique_ptr<nebula_msgs::msg::NebulaPackets> 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<const drivers::continental_ars548::ContinentalARS548SensorConfiguration>
     config_ptr_{};
 
-  rclcpp::Subscription<nebula_msgs::msg::NebulaPackets>::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

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<const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> &
     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<sensor_msgs::msg::PointCloud2>();
+    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,7 @@ 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));
+    detection_list_pub_->publish(*msg);
   }
 
   std::shared_lock lock_cfg(mtx_config_ptr_);
@@ -245,7 +246,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<sensor_msgs::msg::PointCloud2>();
+    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 +278,7 @@ 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));
+    object_list_pub_->publish(*msg);
   }
   objects_rate_bound_status_.tick();
 }
@@ -448,7 +449,7 @@ 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));
+    packets_pub_->publish(*msg);
   }
 }
 

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<const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> &
     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;

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)
 {
@@ -59,13 +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_msgs::msg::NebulaPackets>(
-      "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 " << packets_sub_->get_topic_name());
+      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
@@ -161,7 +162,7 @@ Status ContinentalARS548RosWrapper::validate_and_set_config(
 }
 
 void ContinentalARS548RosWrapper::receive_packets_callback(
-  std::unique_ptr<nebula_msgs::msg::NebulaPackets> packets_msg_ptr)
+  NEBULA_MESSAGE_CONST_SHARED_PTR(nebula_msgs::msg::NebulaPackets) packets_msg_ptr)
 {
   if (hw_interface_wrapper_) {
     RCLCPP_ERROR_THROTTLE(
@@ -171,10 +172,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_msgs::msg::NebulaPacket>();
     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));
   }