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)); } 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_;