From 6b74fbc01abb9154ad42a4942dd8894f3fc2239b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 30 May 2024 12:17:07 +0900 Subject: [PATCH] feat(continental_ars548): combined continental ars548 ros wrappers into a single node (#151) * feat: refactored the ars548 into the new single node scheme Signed-off-by: Kenzo Lobos-Tsunekawa * chore: cleaned code and removed unused code/includes Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed copyright and deleted stray PandarScan Signed-off-by: Kenzo Lobos-Tsunekawa * chore: replaced the class names from Ars548 to ARS548 Signed-off-by: Kenzo Lobos-Tsunekawa * chore: forgot one struct to rename and deleted one unused struct Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 379 ++++++------ .../decoders/continental_ars548_decoder.hpp | 60 +- .../decoders/continental_packets_decoder.hpp | 17 +- .../decoders/continental_ars548_decoder.cpp | 86 ++- nebula_hw_interfaces/CMakeLists.txt | 1 - .../nebula_hw_interface_base.hpp | 5 +- .../continental_ars548_hw_interface.hpp | 119 ++-- .../multi_continental_ars548_hw_interface.hpp | 179 ------ .../continental_ars548_hw_interface.cpp | 213 ++----- .../multi_continental_ars548_hw_interface.cpp | 453 -------------- nebula_ros/CMakeLists.txt | 30 +- ...=> continental_ars548_decoder_wrapper.hpp} | 152 ++--- ...ntinental_ars548_hw_interface_wrapper.hpp} | 141 ++--- .../continental_ars548_ros_wrapper.hpp | 102 ++++ ...nental_ars548_hw_interface_ros_wrapper.hpp | 146 ----- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 2 +- .../velodyne/velodyne_ros_wrapper.hpp | 2 +- .../launch/continental_launch_all_hw.xml | 59 +- ...=> continental_ars548_decoder_wrapper.cpp} | 300 ++++------ ...nental_ars548_hw_interface_ros_wrapper.cpp | 561 ------------------ ...ontinental_ars548_hw_interface_wrapper.cpp | 287 +++++++++ .../continental_ars548_ros_wrapper.cpp | 352 +++++++++++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 356 ----------- .../continental_ros_decoder_test_ars548.cpp | 8 +- .../continental_ros_decoder_test_ars548.hpp | 2 +- ...ntinental_ros_decoder_test_main_ars548.cpp | 2 +- 26 files changed, 1353 insertions(+), 2661 deletions(-) delete mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp delete mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp rename nebula_ros/include/nebula_ros/continental/{continental_ars548_decoder_ros_wrapper.hpp => continental_ars548_decoder_wrapper.hpp} (68%) rename nebula_ros/include/nebula_ros/continental/{continental_ars548_hw_interface_ros_wrapper.hpp => continental_ars548_hw_interface_wrapper.hpp} (51%) create mode 100644 nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp delete mode 100644 nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp rename nebula_ros/src/continental/{continental_ars548_decoder_ros_wrapper.cpp => continental_ars548_decoder_wrapper.cpp} (72%) delete mode 100644 nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp delete mode 100644 nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index b56fd03c5..dbb5ac6bc 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -17,19 +17,12 @@ #include #include -#include "boost/endian/buffers.hpp" #include -#include +#include #include #include -#include -#include -#include -#include -#include -#include #include #include #include @@ -57,13 +50,6 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase float configuration_vehicle_wheelbase{}; }; -/// @brief struct for Multiple ARS548 sensor configuration -struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration -{ - std::vector sensor_ips{}; - std::vector frame_ids{}; -}; - /// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << /// operator) /// @param os @@ -84,33 +70,6 @@ inline std::ostream & operator<<( return os; } -/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the << -/// operator) -/// @param os -/// @param arg -/// @return stream -inline std::ostream & operator<<( - std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg) -{ - std::stringstream sensor_ips_ss; - sensor_ips_ss << "["; - for (const auto sensor_ip : arg.sensor_ips) { - sensor_ips_ss << sensor_ip << ", "; - } - sensor_ips_ss << "]"; - - std::stringstream frame_ids_ss; - frame_ids_ss << "["; - for (const auto frame_id : arg.frame_ids) { - frame_ids_ss << frame_id << ", "; - } - frame_ids_ss << "]"; - - os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str(); - return os; -} - /// @brief semantic struct of ARS548 Status struct ContinentalARS548Status { @@ -327,159 +286,159 @@ struct HeaderPacket struct HeaderSomeIPPacket { - big_uint16_buf_t client_id; - big_uint16_buf_t session_id; - uint8_t protocol_version; - uint8_t interface_version; - uint8_t message_type; - uint8_t return_code; + big_uint16_buf_t client_id{}; + big_uint16_buf_t session_id{}; + uint8_t protocol_version{}; + uint8_t interface_version{}; + uint8_t message_type{}; + uint8_t return_code{}; }; struct HeaderE2EP07Packet { - big_uint64_buf_t crc; - big_uint32_buf_t length; - big_uint32_buf_t sqc; - big_uint32_buf_t data_id; + big_uint64_buf_t crc{}; + big_uint32_buf_t length{}; + big_uint32_buf_t sqc{}; + big_uint32_buf_t data_id{}; }; struct StampSyncStatusPacket { - big_uint32_buf_t timestamp_nanoseconds; - big_uint32_buf_t timestamp_seconds; - uint8_t timestamp_sync_status; + big_uint32_buf_t timestamp_nanoseconds{}; + big_uint32_buf_t timestamp_seconds{}; + uint8_t timestamp_sync_status{}; }; struct DetectionPacket { - big_float32_buf_t azimuth_angle; - big_float32_buf_t azimuth_angle_std; - uint8_t invalid_flags; - big_float32_buf_t elevation_angle; - big_float32_buf_t elevation_angle_std; - big_float32_buf_t range; - big_float32_buf_t range_std; - big_float32_buf_t range_rate; - big_float32_buf_t range_rate_std; - int8_t rcs; - big_uint16_buf_t measurement_id; - uint8_t positive_predictive_value; - uint8_t classification; - uint8_t multi_target_probability; - big_uint16_buf_t object_id; - uint8_t ambiguity_flag; - big_uint16_buf_t sort_index; + big_float32_buf_t azimuth_angle{}; + big_float32_buf_t azimuth_angle_std{}; + uint8_t invalid_flags{}; + big_float32_buf_t elevation_angle{}; + big_float32_buf_t elevation_angle_std{}; + big_float32_buf_t range{}; + big_float32_buf_t range_std{}; + big_float32_buf_t range_rate{}; + big_float32_buf_t range_rate_std{}; + int8_t rcs{}; + big_uint16_buf_t measurement_id{}; + uint8_t positive_predictive_value{}; + uint8_t classification{}; + uint8_t multi_target_probability{}; + big_uint16_buf_t object_id{}; + uint8_t ambiguity_flag{}; + big_uint16_buf_t sort_index{}; }; struct DetectionListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - big_uint16_buf_t origin_invalid_flags; - big_float32_buf_t origin_x_pos; - big_float32_buf_t origin_x_std; - big_float32_buf_t origin_y_pos; - big_float32_buf_t origin_y_std; - big_float32_buf_t origin_z_pos; - big_float32_buf_t origin_z_std; - big_float32_buf_t origin_roll; - big_float32_buf_t origin_roll_std; - big_float32_buf_t origin_pitch; - big_float32_buf_t origin_pitch_std; - big_float32_buf_t origin_yaw; - big_float32_buf_t origin_yaw_std; - uint8_t list_invalid_flags; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + big_uint16_buf_t origin_invalid_flags{}; + big_float32_buf_t origin_x_pos{}; + big_float32_buf_t origin_x_std{}; + big_float32_buf_t origin_y_pos{}; + big_float32_buf_t origin_y_std{}; + big_float32_buf_t origin_z_pos{}; + big_float32_buf_t origin_z_std{}; + big_float32_buf_t origin_roll{}; + big_float32_buf_t origin_roll_std{}; + big_float32_buf_t origin_pitch{}; + big_float32_buf_t origin_pitch_std{}; + big_float32_buf_t origin_yaw{}; + big_float32_buf_t origin_yaw_std{}; + uint8_t list_invalid_flags{}; DetectionPacket detections[MAX_DETECTIONS]; - big_float32_buf_t list_rad_vel_domain_min; - big_float32_buf_t list_rad_vel_domain_max; - big_uint32_buf_t number_of_detections; - big_float32_buf_t alignment_azimuth_correction; - big_float32_buf_t alignment_elevation_correction; - uint8_t alignment_status; + big_float32_buf_t list_rad_vel_domain_min{}; + big_float32_buf_t list_rad_vel_domain_max{}; + big_uint32_buf_t number_of_detections{}; + big_float32_buf_t alignment_azimuth_correction{}; + big_float32_buf_t alignment_elevation_correction{}; + uint8_t alignment_status{}; uint8_t reserved[14]; }; struct ObjectPacket { - big_uint16_buf_t status_sensor; - big_uint32_buf_t id; - big_uint16_buf_t age; - uint8_t status_measurement; - uint8_t status_movement; - big_uint16_buf_t position_invalid_flags; - uint8_t position_reference; - big_float32_buf_t position_x; - big_float32_buf_t position_x_std; - big_float32_buf_t position_y; - big_float32_buf_t position_y_std; - big_float32_buf_t position_z; - big_float32_buf_t position_z_std; - big_float32_buf_t position_covariance_xy; - big_float32_buf_t position_orientation; - big_float32_buf_t position_orientation_std; - uint8_t existence_invalid_flags; - big_float32_buf_t existence_probability; - big_float32_buf_t existence_ppv; - uint8_t classification_car; - uint8_t classification_truck; - uint8_t classification_motorcycle; - uint8_t classification_bicycle; - uint8_t classification_pedestrian; - uint8_t classification_animal; - uint8_t classification_hazard; - uint8_t classification_unknown; - uint8_t classification_overdrivable; - uint8_t classification_underdrivable; - uint8_t dynamics_abs_vel_invalid_flags; - big_float32_buf_t dynamics_abs_vel_x; - big_float32_buf_t dynamics_abs_vel_x_std; - big_float32_buf_t dynamics_abs_vel_y; - big_float32_buf_t dynamics_abs_vel_y_std; - big_float32_buf_t dynamics_abs_vel_covariance_xy; - uint8_t dynamics_rel_vel_invalid_flags; - big_float32_buf_t dynamics_rel_vel_x; - big_float32_buf_t dynamics_rel_vel_x_std; - big_float32_buf_t dynamics_rel_vel_y; - big_float32_buf_t dynamics_rel_vel_y_std; - big_float32_buf_t dynamics_rel_vel_covariance_xy; - uint8_t dynamics_abs_accel_invalid_flags; - big_float32_buf_t dynamics_abs_accel_x; - big_float32_buf_t dynamics_abs_accel_x_std; - big_float32_buf_t dynamics_abs_accel_y; - big_float32_buf_t dynamics_abs_accel_y_std; - big_float32_buf_t dynamics_abs_accel_covariance_xy; - uint8_t dynamics_rel_accel_invalid_flags; - big_float32_buf_t dynamics_rel_accel_x; - big_float32_buf_t dynamics_rel_accel_x_std; - big_float32_buf_t dynamics_rel_accel_y; - big_float32_buf_t dynamics_rel_accel_y_std; - big_float32_buf_t dynamics_rel_accel_covariance_xy; - uint8_t dynamics_orientation_invalid_flags; - big_float32_buf_t dynamics_orientation_rate_mean; - big_float32_buf_t dynamics_orientation_rate_std; - big_uint32_buf_t shape_length_status; - uint8_t shape_length_edge_invalid_flags; - big_float32_buf_t shape_length_edge_mean; - big_float32_buf_t shape_length_edge_std; - big_uint32_buf_t shape_width_status; - uint8_t shape_width_edge_invalid_flags; - big_float32_buf_t shape_width_edge_mean; - big_float32_buf_t shape_width_edge_std; + big_uint16_buf_t status_sensor{}; + big_uint32_buf_t id{}; + big_uint16_buf_t age{}; + uint8_t status_measurement{}; + uint8_t status_movement{}; + big_uint16_buf_t position_invalid_flags{}; + uint8_t position_reference{}; + big_float32_buf_t position_x{}; + big_float32_buf_t position_x_std{}; + big_float32_buf_t position_y{}; + big_float32_buf_t position_y_std{}; + big_float32_buf_t position_z{}; + big_float32_buf_t position_z_std{}; + big_float32_buf_t position_covariance_xy{}; + big_float32_buf_t position_orientation{}; + big_float32_buf_t position_orientation_std{}; + uint8_t existence_invalid_flags{}; + big_float32_buf_t existence_probability{}; + big_float32_buf_t existence_ppv{}; + uint8_t classification_car{}; + uint8_t classification_truck{}; + uint8_t classification_motorcycle{}; + uint8_t classification_bicycle{}; + uint8_t classification_pedestrian{}; + uint8_t classification_animal{}; + uint8_t classification_hazard{}; + uint8_t classification_unknown{}; + uint8_t classification_overdrivable{}; + uint8_t classification_underdrivable{}; + uint8_t dynamics_abs_vel_invalid_flags{}; + big_float32_buf_t dynamics_abs_vel_x{}; + big_float32_buf_t dynamics_abs_vel_x_std{}; + big_float32_buf_t dynamics_abs_vel_y{}; + big_float32_buf_t dynamics_abs_vel_y_std{}; + big_float32_buf_t dynamics_abs_vel_covariance_xy{}; + uint8_t dynamics_rel_vel_invalid_flags{}; + big_float32_buf_t dynamics_rel_vel_x{}; + big_float32_buf_t dynamics_rel_vel_x_std{}; + big_float32_buf_t dynamics_rel_vel_y{}; + big_float32_buf_t dynamics_rel_vel_y_std{}; + big_float32_buf_t dynamics_rel_vel_covariance_xy{}; + uint8_t dynamics_abs_accel_invalid_flags{}; + big_float32_buf_t dynamics_abs_accel_x{}; + big_float32_buf_t dynamics_abs_accel_x_std{}; + big_float32_buf_t dynamics_abs_accel_y{}; + big_float32_buf_t dynamics_abs_accel_y_std{}; + big_float32_buf_t dynamics_abs_accel_covariance_xy{}; + uint8_t dynamics_rel_accel_invalid_flags{}; + big_float32_buf_t dynamics_rel_accel_x{}; + big_float32_buf_t dynamics_rel_accel_x_std{}; + big_float32_buf_t dynamics_rel_accel_y{}; + big_float32_buf_t dynamics_rel_accel_y_std{}; + big_float32_buf_t dynamics_rel_accel_covariance_xy{}; + uint8_t dynamics_orientation_invalid_flags{}; + big_float32_buf_t dynamics_orientation_rate_mean{}; + big_float32_buf_t dynamics_orientation_rate_std{}; + big_uint32_buf_t shape_length_status{}; + uint8_t shape_length_edge_invalid_flags{}; + big_float32_buf_t shape_length_edge_mean{}; + big_float32_buf_t shape_length_edge_std{}; + big_uint32_buf_t shape_width_status{}; + uint8_t shape_width_edge_invalid_flags{}; + big_float32_buf_t shape_width_edge_mean{}; + big_float32_buf_t shape_width_edge_std{}; }; struct ObjectListPacket { - HeaderPacket header; - HeaderSomeIPPacket header_some_ip; - HeaderE2EP07Packet header_e2ep07; - StampSyncStatusPacket stamp; - big_uint32_buf_t event_data_qualifier; - uint8_t extended_qualifier; - uint8_t number_of_objects; + HeaderPacket header{}; + HeaderSomeIPPacket header_some_ip{}; + HeaderE2EP07Packet header_e2ep07{}; + StampSyncStatusPacket stamp{}; + big_uint32_buf_t event_data_qualifier{}; + uint8_t extended_qualifier{}; + uint8_t number_of_objects{}; ObjectPacket objects[MAX_OBJECTS]; }; @@ -513,24 +472,24 @@ struct StatusConfigurationPacket struct SensorStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t sw_version_major; - uint8_t sw_version_minor; - uint8_t sw_version_patch; - StatusConfigurationPacket status; - uint8_t configuration_counter; - uint8_t longitudinal_velocity_status; - uint8_t longitudinal_acceleration_status; - uint8_t lateral_acceleration_status; - uint8_t yaw_rate_status; - uint8_t steering_angle_status; - uint8_t driving_direction_status; - uint8_t characteristic_speed_status; - uint8_t radar_status; - uint8_t voltage_status; - uint8_t temperature_status; - uint8_t blockage_status; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t sw_version_major{}; + uint8_t sw_version_minor{}; + uint8_t sw_version_patch{}; + StatusConfigurationPacket status{}; + uint8_t configuration_counter{}; + uint8_t longitudinal_velocity_status{}; + uint8_t longitudinal_acceleration_status{}; + uint8_t lateral_acceleration_status{}; + uint8_t yaw_rate_status{}; + uint8_t steering_angle_status{}; + uint8_t driving_direction_status{}; + uint8_t characteristic_speed_status{}; + uint8_t radar_status{}; + uint8_t voltage_status{}; + uint8_t temperature_status{}; + uint8_t blockage_status{}; }; struct ConfigurationPacket @@ -545,17 +504,17 @@ struct ConfigurationPacket struct AccelerationLateralCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; struct AccelerationLongitudinalCoGPacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t acceleration_lateral; + big_float32_buf_t acceleration_lateral{}; uint8_t reserved1[22]; }; @@ -569,51 +528,51 @@ struct CharacteristicSpeedPacket struct DrivingDirectionPacket { - HeaderPacket header; - uint8_t reserved0; - uint8_t driving_direction; + HeaderPacket header{}; + uint8_t reserved0{}; + uint8_t driving_direction{}; uint8_t reserved1[20]; }; struct SteeringAngleFrontAxlePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t steering_angle_front_axle; + big_float32_buf_t steering_angle_front_axle{}; uint8_t reserved1[22]; }; struct VelocityVehiclePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[3]; - big_float32_buf_t velocity_vehicle; + big_float32_buf_t velocity_vehicle{}; uint8_t reserved1[21]; }; struct YawRatePacket { - HeaderPacket header; + HeaderPacket header{}; uint8_t reserved0[6]; - big_float32_buf_t yaw_rate; + big_float32_buf_t yaw_rate{}; uint8_t reserved1[22]; }; struct FilterStatusEntryPacket { - uint8_t active; - uint8_t data_index; - big_float32_buf_t min_value; - big_float32_buf_t max_value; + uint8_t active{}; + uint8_t data_index{}; + big_float32_buf_t min_value{}; + big_float32_buf_t max_value{}; }; struct FilterStatusPacket { - HeaderPacket header; - StampSyncStatusPacket stamp; - uint8_t filter_configuration_counter; - uint8_t detection_sort_index; - uint8_t object_sort_index; + HeaderPacket header{}; + StampSyncStatusPacket stamp{}; + uint8_t filter_configuration_counter{}; + uint8_t detection_sort_index{}; + uint8_t object_sort_index{}; FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index 139ea6d5e..af0c508e3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -41,30 +40,16 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder explicit ContinentalARS548Decoder( - const std::shared_ptr & sensor_configuration); + const std::shared_ptr & sensor_configuration); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus() override; /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag - bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override; - - /// @brief Function for parsing detection lists - /// @param data - /// @return Resulting flag - bool ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); - - /// @brief Function for parsing object lists - /// @param data - /// @return Resulting flag - bool ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header); - - /// @brief Function for parsing sensor status messages - /// @param data - /// @return Resulting flag - bool ParseSensorStatusPacket( - const std::vector & data, const std_msgs::msg::Header & header); + bool ProcessPacket(std::unique_ptr packet_msg) override; /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback @@ -86,17 +71,40 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder Status RegisterSensorStatusCallback( std::function sensor_status_callback); + /// @brief Register function to call when a new sensor status message is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterPacketsCallback( + std::function)> packets_callback); + private: + /// @brief Function for parsing detection lists + /// @param data + /// @return Resulting flag + bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + + /// @brief Function for parsing object lists + /// @param data + /// @return Resulting flag + bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + + /// @brief Function for parsing sensor status messages + /// @param data + /// @return Resulting flag + bool ParseSensorStatusPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + std::function msg)> - detection_list_callback_; + detection_list_callback_{}; std::function msg)> - object_list_callback_; - std::function sensor_status_callback_; + object_list_callback_{}; + std::function sensor_status_callback_{}; + std::function msg)> + nebula_packets_callback_{}; ContinentalARS548Status radar_status_{}; /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr config_ptr_{}; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index e5f0f5efa..8760c11b0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,10 +15,9 @@ #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP -#include "nebula_common/point_types.hpp" +#include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include #include #include @@ -39,10 +38,14 @@ class ContinentalPacketsDecoder virtual ~ContinentalPacketsDecoder() = default; ContinentalPacketsDecoder() = default; - /// @brief Virtual function for parsing NebulaPackets based on packet structure - /// @param nebula_packets + /// @brief Get current status of this driver + /// @return Current status + virtual Status GetStatus() = 0; + + /// @brief Virtual function for parsing a NebulaPacket + /// @param packet_msg /// @return Resulting flag - virtual bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) = 0; + virtual bool ProcessPacket(std::unique_ptr packet_msg) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index fb2a44f60..8423d4a0b 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" - -#include "nebula_common/continental/continental_ars548.hpp" +#include +#include #include #include @@ -26,10 +25,15 @@ namespace drivers namespace continental_ars548 { ContinentalARS548Decoder::ContinentalARS548Decoder( - const std::shared_ptr & + const std::shared_ptr & sensor_configuration) { - sensor_configuration_ = sensor_configuration; + config_ptr_ = sensor_configuration; +} + +Status ContinentalARS548Decoder::GetStatus() +{ + return Status::OK; } Status ContinentalARS548Decoder::RegisterDetectionListCallback( @@ -55,14 +59,17 @@ Status ContinentalARS548Decoder::RegisterSensorStatusCallback( return Status::OK; } -bool ContinentalARS548Decoder::ProcessPackets( - const nebula_msgs::msg::NebulaPackets & nebula_packets) +Status ContinentalARS548Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) { - if (nebula_packets.packets.size() != 1) { - return false; - } + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} - const auto & data = nebula_packets.packets[0].data; +bool ContinentalARS548Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const auto & data = packet_msg->data; if (data.size() < sizeof(HeaderPacket)) { return false; @@ -82,13 +89,13 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseDetectionsListPacket(data, nebula_packets.header); + ParseDetectionsListPacket(*packet_msg); } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } - return ParseObjectsListPacket(data, nebula_packets.header); + ParseObjectsListPacket(*packet_msg); } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { if ( data.size() != SENSOR_STATUS_UDP_PAYLOAD || @@ -96,30 +103,39 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseSensorStatusPacket(data, nebula_packets.header); + ParseSensorStatusPacket(*packet_msg); + } + + // Some messages are not parsed but are still sent to the user (e.g., filters) + if (nebula_packets_callback_) { + auto packets_msg = std::make_unique(); + packets_msg->packets.emplace_back(std::move(*packet_msg)); + packets_msg->header.stamp = packet_msg->stamp; + packets_msg->header.frame_id = config_ptr_->frame_id; + nebula_packets_callback_(std::move(packets_msg)); } return true; } bool ContinentalARS548Decoder::ParseDetectionsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; DetectionListPacket detection_list; - assert(sizeof(DetectionListPacket) == data.size()); + assert(sizeof(DetectionListPacket) == packet_msg.data.size()); - std::memcpy(&detection_list, data.data(), sizeof(DetectionListPacket)); + std::memcpy(&detection_list, packet_msg.data.data(), sizeof(DetectionListPacket)); - msg.header.frame_id = sensor_configuration_->frame_id; + msg.header.frame_id = config_ptr_->frame_id; - if (sensor_configuration_->use_sensor_time) { + if (config_ptr_->use_sensor_time) { msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = header.stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; @@ -222,29 +238,31 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( detection_msg.range_rate_std = detection.range_rate_std.value(); } - detection_list_callback_(std::move(msg_ptr)); + if (detection_list_callback_) { + detection_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalARS548Decoder::ParseObjectsListPacket( - const std::vector & data, const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; ObjectListPacket object_list; - assert(sizeof(ObjectListPacket) == data.size()); + assert(sizeof(ObjectListPacket) == packet_msg.data.size()); - std::memcpy(&object_list, data.data(), sizeof(object_list)); + std::memcpy(&object_list, packet_msg.data.data(), sizeof(object_list)); - msg.header.frame_id = sensor_configuration_->object_frame; + msg.header.frame_id = config_ptr_->object_frame; - if (sensor_configuration_->use_sensor_time) { + if (config_ptr_->use_sensor_time) { msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); } else { - msg.header.stamp = header.stamp; + msg.header.stamp = packet_msg.stamp; } msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; @@ -374,16 +392,18 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); } - object_list_callback_(std::move(msg_ptr)); + if (object_list_callback_) { + object_list_callback_(std::move(msg_ptr)); + } return true; } bool ContinentalARS548Decoder::ParseSensorStatusPacket( - const std::vector & data, [[maybe_unused]] const std_msgs::msg::Header & header) + const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; - std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket)); + std::memcpy(&sensor_status_packet, packet_msg.data.data(), sizeof(SensorStatusPacket)); radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); @@ -613,7 +633,9 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.status_total_count++; radar_status_.radar_invalid_count += sensor_status_packet.radar_status == 2 ? 1 : 0; - sensor_status_callback_(radar_status_); + if (sensor_status_callback_) { + sensor_status_callback_(radar_status_); + } return true; } diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 32893d1f4..7cd8b8982 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -40,7 +40,6 @@ ament_auto_add_library(nebula_hw_interfaces_robosense SHARED ament_auto_add_library(nebula_hw_interfaces_continental SHARED src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp - src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 5b46415e7..f5692f5f5 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -1,10 +1,11 @@ #ifndef NEBULA_HW_INTERFACE_BASE_H #define NEBULA_HW_INTERFACE_BASE_H +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "boost_udp_driver/udp_driver.hpp" +#include #include #include #include @@ -22,7 +23,7 @@ class NebulaHwInterfaceBase * @param buffer Buffer containing the data received from the UDP socket * @return Status::OK if no error occurred. */ - virtual void ReceiveSensorPacketCallback([[maybe_unused]] const std::vector & buffer) {} + virtual void ReceiveSensorPacketCallback([[maybe_unused]] std::vector & buffer) {} // virtual Status RegisterScanCallback( // std::function>>)> scan_callback) = 0; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 3e150b18e..ed578b66c 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,32 +14,15 @@ #ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H -// Have to define macros to silence warnings about deprecated headers being used by -// boost/property_tree/ in some versions of boost. -// See: https://github.com/boostorg/property_tree/issues/51 -#include -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include + #include #include #include #include #include -#include - -#include -#include -#include #include -#include #include #include @@ -50,84 +33,31 @@ namespace drivers namespace continental_ars548 { /// @brief Hardware interface of the Continental ARS548 radar -class ContinentalARS548HwInterface : NebulaHwInterfaceBase +class ContinentalARS548HwInterface { -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer)> - nebula_packets_reception_callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - public: /// @brief Constructor ContinentalARS548HwInterface(); - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(const std::vector & buffer) final; - /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart() final; + Status SensorInterfaceStart(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop() final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + Status SensorInterfaceStop(); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; + std::shared_ptr sensor_configuration); - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function + /// @brief Registering callback + /// @param callback Callback function /// @return Resulting status - Status RegisterScanCallback( - std::function)> scan_callback); + Status RegisterCallback( + std::function)> packet_callback); /// @brief Set the sensor mounting parameters /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates @@ -210,6 +140,35 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); + +private: + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallbackWithSender( + std::vector & buffer, const std::string & sender_ip); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback(std::vector & buffer); + + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_ptr_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; + std::shared_ptr config_ptr_; + std::function)> packet_callback_; + + std::shared_ptr parent_node_logger_ptr_; }; } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp deleted file mode 100644 index b029ae40e..000000000 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -#define NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H -// Have to define macros to silence warnings about deprecated headers being used by -// boost/property_tree/ in some versions of boost. -// See: https://github.com/boostorg/property_tree/issues/51 -#include -#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 -#define BOOST_BIND_GLOBAL_PLACEHOLDERS -#endif -#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 -#define BOOST_ALLOW_DEPRECATED_HEADERS -#endif -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -/// @brief Hardware interface of the Continental ARS548 radar -class MultiContinentalARS548HwInterface : NebulaHwInterfaceBase -{ -private: - std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; - std::vector> sensor_udp_drivers_; - std::shared_ptr sensor_configuration_; - std::unique_ptr nebula_packets_ptr_; - std::function buffer, const std::string & sensor_ip)> - nebula_packets_reception_callback_; - - std::mutex sensor_status_mutex_; - - SensorStatusPacket sensor_status_packet_{}; - FilterStatusPacket filter_status_{}; - std::unordered_map sensor_ip_to_frame_; - - std::shared_ptr parent_node_logger; - - /// @brief Printing the string to RCLCPP_INFO_STREAM - /// @param info Target string - void PrintInfo(std::string info); - - /// @brief Printing the string to RCLCPP_ERROR_STREAM - /// @param error Target string - void PrintError(std::string error); - - /// @brief Printing the string to RCLCPP_DEBUG_STREAM - /// @param debug Target string - void PrintDebug(std::string debug); - - /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM - /// @param bytes Target byte vector - void PrintDebug(const std::vector & bytes); - -public: - /// @brief Constructor - MultiContinentalARS548HwInterface(); - - /// @brief Process a new filter status packet - /// @param buffer The buffer containing the status packet - void ProcessFilterStatusPacket(const std::vector & buffer); - - /// @brief Process a new data packet - /// @param buffer The buffer containing the data packet - void ProcessDataPacket(const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Callback function to receive the Cloud Packet data from the UDP Driver - /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sensor_ip); - - /// @brief Starting the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStart() final; - - /// @brief Function for stopping the interface that handles UDP streams - /// @return Resulting status - Status SensorInterfaceStop() final; - - /// @brief Printing sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; - - /// @brief Setting sensor configuration - /// @param sensor_configuration SensorConfiguration for this interface - /// @return Resulting status - Status SetSensorConfiguration( - std::shared_ptr sensor_configuration) final; - - /// @brief Registering callback for PandarScan - /// @param scan_callback Callback function - /// @return Resulting status - Status RegisterScanCallback( - std::function, const std::string &)> - scan_callback); - - /// @brief Set the current lateral acceleration - /// @param lateral_acceleration Current lateral acceleration - /// @return Resulting status - Status SetAccelerationLateralCog(float lateral_acceleration); - - /// @brief Set the current longitudinal acceleration - /// @param longitudinal_acceleration Current longitudinal acceleration - /// @return Resulting status - Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); - - /// @brief Set the characteristic speed - /// @param characteristic_speed Characteristic speed - /// @return Resulting status - Status SetCharacteristicSpeed(float characteristic_speed); - - /// @brief Set the current direction - /// @param direction Current driving direction - /// @return Resulting status - Status SetDrivingDirection(int direction); - - /// @brief Set the current steering angle - /// @param angle_rad Current steering angle in radians - /// @return Resulting status - Status SetSteeringAngleFrontAxle(float angle_rad); - - /// @brief Set the current vehicle velocity - /// @param velocity Current vehicle velocity - /// @return Resulting status - Status SetVelocityVehicle(float velocity); - - /// @brief Set the current yaw rate - /// @param yaw_rate Current yaw rate - /// @return Resulting status - Status SetYawRate(float yaw_rate); - - /// @brief Checking the current settings and changing the difference point - /// @return Resulting status - Status CheckAndSetConfig(); - - /// @brief Setting rclcpp::Logger - /// @param node Logger - void SetLogger(std::shared_ptr node); -}; -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula - -#endif // NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 2deeb7a28..bf05631b5 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -16,8 +16,6 @@ #include #include -#include -#include namespace nebula { namespace drivers @@ -25,150 +23,80 @@ namespace drivers namespace continental_ars548 { ContinentalARS548HwInterface::ContinentalARS548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)}, - nebula_packets_ptr_{std::make_unique()} +: sensor_io_context_ptr_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_ptr_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_ptr_)} { } Status ContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) + std::shared_ptr new_config_ptr) { - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } - + config_ptr_ = new_config_ptr; return Status::OK; } Status ContinentalARS548HwInterface::SensorInterfaceStart() { try { - sensor_udp_driver_->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - sensor_udp_driver_->receiver()->setMulticast(true); - sensor_udp_driver_->receiver()->open(); - sensor_udp_driver_->receiver()->bind(); - sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( + sensor_udp_driver_ptr_->init_receiver( + config_ptr_->multicast_ip, config_ptr_->data_port, config_ptr_->host_ip, + config_ptr_->data_port, 2 << 16); + sensor_udp_driver_ptr_->receiver()->setMulticast(true); + sensor_udp_driver_ptr_->receiver()->open(); + sensor_udp_driver_ptr_->receiver()->bind(); + sensor_udp_driver_ptr_->receiver()->asyncReceiveWithSender(std::bind( &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); - sensor_udp_driver_->init_sender( - sensor_configuration_->sensor_ip, sensor_configuration_->configuration_sensor_port, - sensor_configuration_->host_ip, sensor_configuration_->configuration_host_port); + sensor_udp_driver_ptr_->init_sender( + config_ptr_->sensor_ip, config_ptr_->configuration_sensor_port, config_ptr_->host_ip, + config_ptr_->configuration_host_port); - sensor_udp_driver_->sender()->open(); - sensor_udp_driver_->sender()->bind(); + sensor_udp_driver_ptr_->sender()->open(); + sensor_udp_driver_ptr_->sender()->bind(); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; + std::cerr << status << config_ptr_->sensor_ip << "," << config_ptr_->data_port << std::endl; return status; } return Status::OK; } -Status ContinentalARS548HwInterface::RegisterScanCallback( - std::function)> callback) +Status ContinentalARS548HwInterface::RegisterCallback( + std::function)> packet_callback) { - nebula_packets_reception_callback_ = std::move(callback); + packet_callback_ = std::move(packet_callback); return Status::OK; } void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( - const std::vector & buffer, const std::string & sender_ip) + std::vector & buffer, const std::string & sender_ip) { - if (sender_ip == sensor_configuration_->sensor_ip) { + if (sender_ip == config_ptr_->sensor_ip) { ReceiveSensorPacketCallback(buffer); } } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); return; } - HeaderPacket header_packet{}; - std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - - if (header_packet.service_id.value() != 0) { - PrintError("Invalid service id"); - return; - } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { - if ( - buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || - header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { - PrintError("SensorStatus message with invalid size"); - return; - } - ProcessDataPacket(buffer); - } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { - if ( - buffer.size() != FILTER_STATUS_UDP_PAYLOAD || - header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { - PrintError("FilterStatus message with invalid size"); - return; - } - - ProcessFilterStatusPacket(buffer); - } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { - if ( - buffer.size() != DETECTION_LIST_UDP_PAYLOAD || - header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { - PrintError("DetectionList message with invalid size"); - return; - } - - ProcessDataPacket(buffer); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } - - ProcessDataPacket(buffer); - } -} - -void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = + const auto now = std::chrono::high_resolution_clock::now(); + const auto timestamp_ns = std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + auto msg_ptr = std::make_unique(); + msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); + msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); + msg_ptr->data.swap(buffer); - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); - nebula_packets_ptr_ = std::make_unique(); + packet_callback_(std::move(msg_ptr)); } Status ContinentalARS548HwInterface::SensorInterfaceStop() @@ -176,15 +104,6 @@ Status ContinentalARS548HwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status ContinentalARS548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - Status ContinentalARS548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) @@ -220,11 +139,11 @@ Status ContinentalARS548HwInterface::SetSensorMounting( PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -259,11 +178,11 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( PrintInfo("height_autosar = " + std::to_string(height_autosar)); PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -303,11 +222,11 @@ Status ContinentalARS548HwInterface::SetRadarParameters( PrintInfo("hcc = " + std::to_string(hcc)); PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -344,11 +263,11 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -375,11 +294,11 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -408,11 +327,11 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit std::memcpy( send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -439,11 +358,11 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic std::vector send_vector(sizeof(CharacteristicSpeedPacket)); std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -475,11 +394,11 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) std::vector send_vector(sizeof(DrivingDirectionPacket)); std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -507,11 +426,11 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) std::memcpy( send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -538,11 +457,11 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) std::vector send_vector(sizeof(VelocityVehiclePacket)); std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } @@ -569,24 +488,24 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) std::vector send_vector(sizeof(YawRatePacket)); std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - if (!sensor_udp_driver_->sender()->isOpen()) { + if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; } - sensor_udp_driver_->sender()->asyncSend(send_vector); + sensor_udp_driver_ptr_->sender()->asyncSend(send_vector); return Status::OK; } void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ptr_ = logger; } void ContinentalARS548HwInterface::PrintInfo(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); } else { std::cout << info << std::endl; } @@ -594,8 +513,8 @@ void ContinentalARS548HwInterface::PrintInfo(std::string info) void ContinentalARS548HwInterface::PrintError(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); } else { std::cerr << error << std::endl; } @@ -603,23 +522,13 @@ void ContinentalARS548HwInterface::PrintError(std::string error) void ContinentalARS548HwInterface::PrintDebug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); } else { std::cout << debug << std::endl; } } -void ContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - } // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp deleted file mode 100644 index 58cd25f09..000000000 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp +++ /dev/null @@ -1,453 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// - -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#include -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ -MultiContinentalARS548HwInterface::MultiContinentalARS548HwInterface() -: sensor_io_context_{new ::drivers::common::IoContext(1)}, - nebula_packets_ptr_{std::make_unique()} -{ -} - -Status MultiContinentalARS548HwInterface::SetSensorConfiguration( - std::shared_ptr sensor_configuration) -{ - Status status = Status::OK; - - try { - sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); - } catch (const std::exception & ex) { - status = Status::SENSOR_CONFIG_ERROR; - std::cerr << status << std::endl; - return status; - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStart() -{ - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_->sensor_ips.size(); - sensor_id++) { - auto udp_driver = std::make_unique<::drivers::udp_driver::UdpDriver>(*sensor_io_context_); - sensor_ip_to_frame_[sensor_configuration_->sensor_ips[sensor_id]] = - sensor_configuration_->frame_ids[sensor_id]; - - try { - if (sensor_id == 0) { - udp_driver->init_receiver( - sensor_configuration_->multicast_ip, sensor_configuration_->data_port, - sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); - udp_driver->receiver()->setMulticast(true); - udp_driver->receiver()->open(); - udp_driver->receiver()->bind(); - udp_driver->receiver()->asyncReceiveWithSender(std::bind( - &MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - udp_driver->init_sender( - sensor_configuration_->sensor_ips[sensor_id], - sensor_configuration_->configuration_sensor_port, sensor_configuration_->host_ip, - sensor_configuration_->configuration_host_port); - - udp_driver->sender()->open(); - udp_driver->sender()->bind(); - - if (!udp_driver->sender()->isOpen()) { - return Status::ERROR_1; - } - } catch (const std::exception & ex) { - Status status = Status::UDP_CONNECTION_ERROR; - std::cerr << status << sensor_configuration_->sensor_ip << "," - << sensor_configuration_->data_port << std::endl; - return status; - } - - sensor_udp_drivers_.emplace_back(std::move(udp_driver)); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::RegisterScanCallback( - std::function, const std::string &)> - callback) -{ - nebula_packets_reception_callback_ = std::move(callback); - return Status::OK; -} - -void MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback( - const std::vector & buffer, const std::string & sender_ip) -{ - if (buffer.size() < sizeof(HeaderPacket)) { - PrintError("Unrecognized packet. Too short"); - return; - } - - HeaderPacket header_packet{}; - std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - - if (header_packet.service_id.value() != 0) { - PrintError("Invalid service id"); - return; - } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { - if ( - buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || - header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { - PrintError("SensorStatus message with invalid size"); - return; - } - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { - if ( - buffer.size() != FILTER_STATUS_UDP_PAYLOAD || - header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { - PrintError("FilterStatus message with invalid size"); - return; - } - - ProcessFilterStatusPacket(buffer); - } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { - if ( - buffer.size() != DETECTION_LIST_UDP_PAYLOAD || - header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { - PrintError("DetectionList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { - if ( - buffer.size() != OBJECT_LIST_UDP_PAYLOAD || - header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { - PrintError("ObjectList message with invalid size"); - return; - } - - ProcessDataPacket(buffer, sender_ip); - } -} - -void MultiContinentalARS548HwInterface::ProcessFilterStatusPacket( - const std::vector & buffer) -{ - assert(buffer.size() == sizeof(FilterStatusPacket)); - std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); -} - -void MultiContinentalARS548HwInterface::ProcessDataPacket( - const std::vector & buffer, const std::string & sensor_ip) -{ - nebula_msgs::msg::NebulaPacket nebula_packet; - nebula_packet.data = buffer; - auto now = std::chrono::system_clock::now(); - auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); - auto now_nanosecs = - std::chrono::duration_cast(now.time_since_epoch()).count(); - nebula_packet.stamp.sec = static_cast(now_secs); - nebula_packet.stamp.nanosec = - static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); - nebula_packets_ptr_->packets.emplace_back(nebula_packet); - - nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; - nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; - - nebula_packets_reception_callback_(std::move(nebula_packets_ptr_), sensor_ip); - nebula_packets_ptr_ = std::make_unique(); -} - -Status MultiContinentalARS548HwInterface::SensorInterfaceStop() -{ - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::GetSensorConfiguration( - SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - PrintDebug(ss.str()); - return Status::ERROR_1; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) -{ - constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; - constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; - - if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { - PrintError("Invalid lateral_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLateralCoGPacket acceleration_lateral_cog{}; - static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); - acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; - acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; - acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; - acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; - - std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); - std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetAccelerationLongitudinalCog( - float longitudinal_acceleration) -{ - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; - constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; - - if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { - PrintError("Invalid longitudinal_acceleration value"); - return Status::ERROR_1; - } - - AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; - static_assert( - sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); - acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; - acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; - acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; - - std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); - std::memcpy( - send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) -{ - constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; - constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; - constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; - - if (characteristic_speed < 0.f || characteristic_speed > 255.f) { - PrintError("Invalid characteristic_speed value"); - return Status::ERROR_1; - } - - CharacteristicSpeedPacket characteristic_speed_packet{}; - static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); - characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; - characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; - characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; - characteristic_speed_packet.characteristic_speed = characteristic_speed; - - std::vector send_vector(sizeof(CharacteristicSpeedPacket)); - std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetDrivingDirection(int direction) -{ - constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; - constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; - constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; - constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; - constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; - constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_UDP_LENGTH = 30; - - DrivingDirectionPacket driving_direction_packet{}; - static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); - driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; - driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; - driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; - - if (direction == 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; - } else if (direction > 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; - } else { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; - } - - std::vector send_vector(sizeof(DrivingDirectionPacket)); - std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) -{ - constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; - constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; - constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_UDP_LENGTH = 40; - - if (angle_rad < -90.f || angle_rad > 90.f) { - PrintError("Invalid angle_rad value"); - return Status::ERROR_1; - } - - SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); - steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; - steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; - - std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); - std::memcpy( - send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) -{ - if (velocity_kmh < 0.f || velocity_kmh > 350.f) { - PrintError("Invalid velocity value"); - return Status::ERROR_1; - } - - constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; - constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; - constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_UDP_SIZE = 36; - - VelocityVehiclePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); - steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; - steering_angle_front_axle_packet.velocity_vehicle = velocity_kmh; - - std::vector send_vector(sizeof(VelocityVehiclePacket)); - std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -Status MultiContinentalARS548HwInterface::SetYawRate(float yaw_rate) -{ - if (yaw_rate < -163.83 || yaw_rate > 163.83) { - PrintError("Invalid yaw rate value"); - return Status::ERROR_1; - } - - constexpr uint16_t YAW_RATE_SERVICE_ID = 0; - constexpr uint16_t YAW_RATE_METHOD_ID = 326; - constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_UDP_SIZE = 40; - - YawRatePacket yaw_rate_packet{}; - static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); - yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; - yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; - yaw_rate_packet.header.length = YAW_RATE_LENGTH; - yaw_rate_packet.yaw_rate = yaw_rate; - - std::vector send_vector(sizeof(YawRatePacket)); - std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); - - for (auto & udp_driver : sensor_udp_drivers_) { - udp_driver->sender()->asyncSend(send_vector); - } - - return Status::OK; -} - -void MultiContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) -{ - parent_node_logger = logger; -} - -void MultiContinentalARS548HwInterface::PrintInfo(std::string info) -{ - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); - } else { - std::cout << info << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintError(std::string error) -{ - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); - } else { - std::cerr << error << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(std::string debug) -{ - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); - } else { - std::cout << debug << std::endl; - } -} - -void MultiContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) -{ - std::stringstream ss; - for (const auto & b : bytes) { - ss << static_cast(b) << ", "; - } - ss << std::endl; - PrintDebug(ss.str()); -} - -} // namespace continental_ars548 -} // namespace drivers -} // namespace nebula diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index be5377e91..c8723cabb 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -78,30 +78,16 @@ rclcpp_components_register_node(robosense_ros_wrapper ) ## Continental -# Hw Interface -ament_auto_add_library(continental_ars548_hw_ros_wrapper SHARED - src/continental/continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_hw_ros_wrapper - PLUGIN "ContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node - ) - -ament_auto_add_library(multi_continental_ars548_hw_ros_wrapper SHARED - src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp - ) -rclcpp_components_register_node(multi_continental_ars548_hw_ros_wrapper - PLUGIN "MultiContinentalARS548HwInterfaceRosWrapper" - EXECUTABLE multi_continental_ars548_hw_interface_ros_wrapper_node +ament_auto_add_library(continental_ars548_ros_wrapper SHARED + src/continental/continental_ars548_ros_wrapper.cpp + src/continental/continental_ars548_decoder_wrapper.cpp + src/continental/continental_ars548_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp ) -# DriverDecoder -ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED - src/continental/continental_ars548_decoder_ros_wrapper.cpp - ) -rclcpp_components_register_node(continental_ars548_driver_ros_wrapper - PLUGIN "ContinentalARS548DriverRosWrapper" - EXECUTABLE continental_ars548_driver_ros_wrapper_node +rclcpp_components_register_node(continental_ars548_ros_wrapper + PLUGIN "ContinentalARS548RosWrapper" + EXECUTABLE continental_ars548_ros_wrapper_node ) if(BUILD_TESTING) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp similarity index 68% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 059388639..f975a8fec 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,18 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548DriverRosWrapper_H -#define NEBULA_ContinentalARS548DriverRosWrapper_H +#pragma once -#include #include #include -#include +#include #include -#include -#include +#include +#include #include -#include #include #include @@ -34,75 +31,38 @@ #include #include #include +#include #include -#include #include +#include #include +#include namespace nebula { namespace ros { -/// @brief Ros wrapper of continental radar ethernet driver -class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase +class ContinentalARS548DecoderWrapper { - std::shared_ptr decoder_ptr_; - Status wrapper_status_; - - rclcpp::Subscription::SharedPtr packets_sub_; - - 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 scan_raw_pub_; - rclcpp::Publisher::SharedPtr objects_raw_pub_; - rclcpp::Publisher::SharedPtr objects_markers_pub_; - rclcpp::Publisher::SharedPtr diagnostics_pub_; - - std::unordered_set previous_ids_; - - constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { - {{{-1.0, -1.0}}, - {{-1.0, 0.0}}, - {{-1.0, 1.0}}, - {{0.0, 1.0}}, - {{1.0, 1.0}}, - {{1.0, 0.0}}, - {{1.0, -1.0}}, - {{0.0, -1.0}}, - {{0.0, 0.0}}}}; - - std::shared_ptr - sensor_cfg_ptr_; +public: + ContinentalARS548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config, + bool launch_hw); - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; + void ProcessPacket(std::unique_ptr packet_msg); - /// @brief Initializing ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @param calibration_configuration CalibrationConfiguration for this driver - /// @return Resulting status - Status InitializeDriver(std::shared_ptr sensor_configuration); + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config); - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @param calibration_configuration Output of CalibrationConfiguration - /// @param correction_configuration Output of CorrectionConfiguration (for AT) - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); - /// @brief Convert seconds to chrono::nanoseconds - /// @param seconds - /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) - { - return std::chrono::duration_cast( - std::chrono::duration(seconds)); - } + nebula::Status Status(); /// @brief Callback to process new ContinentalArs548DetectionList from the driver /// @param msg The new ContinentalArs548DetectionList from the driver @@ -118,16 +78,14 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive void SensorStatusCallback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status); -public: - explicit ContinentalARS548DriverRosWrapper(const rclcpp::NodeOptions & options); - - /// @brief Callback for NebulaPackets subscriber - /// @param scan_msg Received NebulaPackets - void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + /// @brief Callback to process new ContinentalARS548Status from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void PacketsCallback(std::unique_ptr msg); - /// @brief Get current status of this driver - /// @return Current status - Status GetStatus(); +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config); /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg @@ -158,9 +116,53 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @return Resulting MarkerArray msg visualization_msgs::msg::MarkerArray ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg); -}; + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + config_ptr_{}; + + std::shared_ptr driver_ptr_{}; + std::mutex mtx_driver_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 scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + rclcpp::Publisher::SharedPtr diagnostics_pub_{}; + + std::unordered_set previous_ids_; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { + {{{-1.0, -1.0}}, + {{-1.0, 0.0}}, + {{-1.0, 1.0}}, + {{0.0, 1.0}}, + {{1.0, 1.0}}, + {{1.0, 0.0}}, + {{1.0, -1.0}}, + {{0.0, -1.0}}, + {{0.0, 0.0}}}}; + + std::shared_ptr cloud_watchdog_; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp similarity index 51% rename from nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index f9b66e951..071b67126 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,16 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#pragma once -#include -#include -#include +#include #include -#include +#include #include -#include #include #include @@ -29,77 +25,35 @@ #include #include #include -#include -#include -#include -#include - -#include #include -#include -#include -#include -#include namespace nebula { namespace ros { -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase +class ContinentalARS548HwInterfaceWrapper { - drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; - Status interface_status_; +public: + ContinentalARS548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration_; + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); - /// @brief Received Continental Radar message publisher - rclcpp::Publisher::SharedPtr packets_pub_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; + void OnConfigChange( + const std::shared_ptr & + new_config_ptr); - bool standstill_{true}; + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); - 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_; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + std::shared_ptr HwInterface() const; +private: /// @brief Callback to send the odometry information to the radar device /// @param msg The odometry message void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); @@ -148,41 +102,28 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, const std::shared_ptr response); -public: - explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_{}; + rclcpp::Logger logger_; + nebula::Status status_{}; + std::shared_ptr + config_ptr_{}; -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; + rclcpp::Subscription::SharedPtr odometry_sub_{}; + rclcpp::Subscription::SharedPtr + acceleration_sub_{}; + rclcpp::Subscription::SharedPtr steering_angle_sub_{}; + 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_{}; +}; } // namespace ros } // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp new file mode 100644 index 000000000..1bd88ec75 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -0,0 +1,102 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental ars548 driver +class ContinentalARS548RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalARS548RosWrapper() noexcept {}; + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); + + /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart(); + +private: + /// @brief Callback from the hw interface's raw data + void ReceivePacketCallback(std::unique_ptr msg_ptr); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + + /// @brief Retrieve the parameters from ROS and set the driver and hw interface + /// @return Resulting status + Status DeclareAndGetSensorConfigParams(); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + Status ValidateAndSetConfig( + std::shared_ptr & + new_config); + + Status wrapper_status_; + + std::shared_ptr + config_ptr_{}; + + /// @brief Stores received packets that have not been processed yet by the decoder thread + mt_queue> packet_queue_; + /// @brief Thread to isolate decoding from receiving + std::thread decoder_thread_; + + rclcpp::Subscription::SharedPtr packets_sub_{}; + + bool launch_hw_{}; + + std::optional hw_interface_wrapper_{}; + std::optional decoder_wrapper_{}; + + std::mutex mtx_config_; + + OnSetParametersCallbackHandle::SharedPtr parameter_event_cb_{}; +}; + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp deleted file mode 100644 index 19429b99c..000000000 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ /dev/null @@ -1,146 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace nebula -{ -namespace ros -{ -/// @brief Get parameter from rclcpp::Parameter -/// @tparam T -/// @param p Parameter from rclcpp parameter callback -/// @param name Target parameter name -/// @param value Corresponding value -/// @return Whether the target name existed -template -bool get_param(const std::vector & p, const std::string & name, T & value) -{ - auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { - return parameter.get_name() == name; - }); - if (it != p.cend()) { - value = it->template get_value(); - return true; - } - return false; -} - -/// @brief Hardware interface ros wrapper of continental radar ethernet driver -/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices -/// The reason behind this is a not-so-efficient multicasting and package processing when having N -/// interfaces for N devices If we end up having problems because of that we may switch to this -/// implementation. Otherwise this implementation will be removed at a later date -class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase -{ - drivers::continental_ars548::MultiContinentalARS548HwInterface hw_interface_; - Status interface_status_; - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration sensor_configuration_; - - /// @brief Received Continental Radar message publisher - std::unordered_map::SharedPtr> - packets_pub_map_; - rclcpp::Subscription::SharedPtr odometry_sub_; - rclcpp::Subscription::SharedPtr acceleration_sub_; - rclcpp::Subscription::SharedPtr steering_angle_sub_; - - bool standstill_{true}; - - /// @brief Initializing hardware interface ros wrapper - /// @param sensor_configuration SensorConfiguration for this driver - /// @return Resulting status - Status InitializeHwInterface( - const drivers::SensorConfigurationBase & sensor_configuration) override; - /// @brief Callback for receiving NebulaPackets - /// @param packets_buffer Received NebulaPackets - void ReceivePacketsDataCallback( - std::unique_ptr packets_buffer, const std::string & sensor_ip); - - /// @brief Callback to send the odometry information to the radar device - /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the acceleration information to the radar device - /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - - /// @brief Callback to send the steering angle information to the radar device - /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); - -public: - explicit MultiContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~MultiContinentalARS548HwInterfaceRosWrapper() noexcept override; - - /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) - /// @return Resulting status - Status StreamStart() override; - /// @brief Stop point cloud streaming (not used) - /// @return Resulting status - Status StreamStop() override; - /// @brief Shutdown (not used) - /// @return Resulting status - Status Shutdown() override; - /// @brief Get configurations from ros parameters - /// @param sensor_configuration Output of SensorConfiguration - /// @return Resulting status - Status GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration); - -private: - std::mutex mtx_config_; - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /// @brief rclcpp parameter callback - /// @param parameters Received parameters - /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult paramCallback( - const std::vector & parameters); - - /// @brief Updating rclcpp parameter - /// @return SetParametersResult - std::vector updateParameters(); -}; - -} // namespace ros -} // namespace nebula - -#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 0a60d3e8e..63ce94915 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -45,7 +45,7 @@ class HesaiRosWrapper final : public rclcpp::Node /// @return Current status Status GetStatus(); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart(); diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 93cfc5b0c..5b8d17697 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -46,7 +46,7 @@ class VelodyneRosWrapper final : public rclcpp::Node /// @return Current status Status GetStatus(); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart(); diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index d5ed36078..31798493a 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -26,29 +26,14 @@ - - - - - - - - - + - - - - - - - + @@ -69,44 +54,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp similarity index 72% rename from nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp rename to nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index ecad7752f..80f653d04 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -20,222 +20,113 @@ namespace nebula { namespace ros { -ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() +ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr, + bool launch_hw) +: status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalARS548Decoder")), + config_ptr_(config_ptr) { - drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; + using std::chrono_literals::operator""us; + if (!config_ptr) { + throw std::runtime_error( + "ContinentalARS548DecoderWrapper cannot be instantiated without a valid config!"); + } - setvbuf(stdout, NULL, _IONBF, BUFSIZ); + RCLCPP_INFO(logger_, "Starting Decoder"); - hw_interface_.SetLogger(std::make_shared(this->get_logger())); + InitializeDriver(config_ptr); + status_ = driver_ptr_->GetStatus(); - wrapper_status_ = GetParameters(sensor_configuration); - if (Status::OK != wrapper_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); - return; + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); - - sensor_cfg_ptr_ = - std::make_shared( - sensor_configuration); - wrapper_status_ = InitializeDriver( - std::const_pointer_cast( - sensor_cfg_ptr_)); + // Publish packets only if HW interface is connected + if (launch_hw) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); detection_list_pub_ = - this->create_publisher( + parent_node->create_publisher( "continental_detections", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "continental_objects", rclcpp::SensorDataQoS()); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", rclcpp::SensorDataQoS()); - detection_pointcloud_pub_ = this->create_publisher( + detection_pointcloud_pub_ = parent_node->create_publisher( "detection_points", rclcpp::SensorDataQoS()); object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); + parent_node->create_publisher("object_points", pointcloud_qos); scan_raw_pub_ = - this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("scan_raw", pointcloud_qos); objects_raw_pub_ = - this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + parent_node->create_publisher("objects_raw", pointcloud_qos); objects_markers_pub_ = - this->create_publisher("marker_array", 10); + parent_node->create_publisher("marker_array", 10); diagnostics_pub_ = - this->create_publisher("diagnostics", 10); -} + parent_node->create_publisher("diagnostics", 10); -void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + cloud_watchdog_ = + std::make_shared(*parent_node, 100'000us, [this, parent_node](bool ok) { + if (ok) return; + RCLCPP_WARN_THROTTLE( + logger_, *parent_node->get_clock(), 5000, "Missed pointcloud output deadline"); + }); } -Status ContinentalARS548DriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) +Status ContinentalARS548DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config) { - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalARS548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalARS548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterSensorStatusCallback(std::bind( - &ContinentalARS548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); + + driver_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalARS548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalARS548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterSensorStatusCallback( + std::bind(&ContinentalARS548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalARS548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); return Status::OK; } -Status ContinentalARS548DriverRosWrapper::GetStatus() +void ContinentalARS548DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config_ptr) { - return wrapper_status_; + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config_ptr); + config_ptr_ = new_config_ptr; } -Status ContinentalARS548DriverRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +void ContinentalARS548DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) { - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration); + driver_ptr_->ProcessPacket(std::move(packet_msg)); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; + cloud_watchdog_->update(); } -void ContinentalARS548DriverRosWrapper::DetectionListCallback( +void ContinentalARS548DecoderWrapper::DetectionListCallback( std::unique_ptr msg) { if ( @@ -264,7 +155,7 @@ void ContinentalARS548DriverRosWrapper::DetectionListCallback( } } -void ContinentalARS548DriverRosWrapper::ObjectListCallback( +void ContinentalARS548DecoderWrapper::ObjectListCallback( std::unique_ptr msg) { if ( @@ -300,20 +191,20 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } -void ContinentalARS548DriverRosWrapper::SensorStatusCallback( +void ContinentalARS548DecoderWrapper::SensorStatusCallback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status) { diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; - diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; + diagnostic_array_msg.header.frame_id = config_ptr_->frame_id; diagnostic_array_msg.status.resize(1); auto & status = diagnostic_array_msg.status[0]; status.values.reserve(36); status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - status.hardware_id = sensor_cfg_ptr_->frame_id; - status.name = sensor_cfg_ptr_->frame_id; + status.hardware_id = config_ptr_->frame_id; + status.name = config_ptr_->frame_id; status.message = "Diagnostic messages from ARS548"; auto add_diagnostic = [&status](const std::string & key, const std::string & value) { @@ -414,8 +305,18 @@ void ContinentalARS548DriverRosWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } +void ContinentalARS548DecoderWrapper::PacketsCallback( + std::unique_ptr msg) +{ + if ( + packets_pub_ && (packets_pub_->get_subscription_count() > 0 || + packets_pub_->get_intra_process_subscription_count() > 0)) { + packets_pub_->publish(std::move(msg)); + } +} + pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -455,7 +356,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( } pcl::PointCloud::Ptr -ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -494,7 +395,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -520,7 +421,7 @@ radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan return output_msg; } -radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -629,7 +530,7 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToMarkers( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; @@ -692,7 +593,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + box_marker.header.frame_id = config_ptr_->object_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -773,7 +674,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; + delete_marker.header.frame_id = config_ptr_->object_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; @@ -797,6 +698,15 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT return marker_array; } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548DriverRosWrapper) +nebula::Status ContinentalARS548DecoderWrapper::Status() +{ + std::lock_guard lock(mtx_driver_ptr_); + + if (!driver_ptr_) { + return nebula::Status::NOT_INITIALIZED; + } + + return driver_ptr_->GetStatus(); +} } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index d7baa3f6a..000000000 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,561 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - hw_interface_.RegisterScanCallback(std::bind( - &ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1)); - packets_pub_ = this->create_publisher( - "nebula_packets", rclcpp::SensorDataQoS()); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - - set_network_configuration_service_server_ = - this->create_service( - "set_network_configuration", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_sensor_mounting_service_server_ = - this->create_service( - "set_sensor_mounting", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_vehicle_parameters_service_server_ = - this->create_service( - "set_vehicle_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_radar_parameters_service_server_ = - this->create_service( - "set_radar_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - return interface_status_; -} - -Status ContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status ContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_length", descriptor); - sensor_configuration.configuration_vehicle_length = - static_cast(this->get_parameter("configuration_vehicle_length").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_width", descriptor); - sensor_configuration.configuration_vehicle_width = - static_cast(this->get_parameter("configuration_vehicle_width").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_height", descriptor); - sensor_configuration.configuration_vehicle_height = - static_cast(this->get_parameter("configuration_vehicle_height").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_vehicle_wheelbase", descriptor); - sensor_configuration.configuration_vehicle_wheelbase = - static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer) -{ - packets_pub_->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::ContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | - get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | - get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | - get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( - const std::shared_ptr request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - - auto tf_buffer = std::make_unique(this->get_clock()); - auto tf_listener = std::make_unique(*tf_buffer); - - float longitudinal = request->longitudinal; - float lateral = request->lateral; - float vertical = request->vertical; - float yaw = request->yaw; - float pitch = request->pitch; - - if (request->autoconfigure_extrinsics) { - RCLCPP_INFO( - this->get_logger(), - "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " - "tfs"); - - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; - } - - const auto & quat = base_to_sensor_tf.transform.rotation; - geometry_msgs::msg::Vector3 rpy; - tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - - longitudinal = base_to_sensor_tf.transform.translation.x - - sensor_configuration_.configuration_vehicle_wheelbase; - lateral = base_to_sensor_tf.transform.translation.y; - vertical = base_to_sensor_tf.transform.translation.z; - yaw = rpy.z; - pitch = rpy.y; - } - - auto result = hw_interface_.SetSensorMounting( - longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr< - continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> - request, - const std::shared_ptr - response) -{ - float vehicle_length = request->vehicle_length; - float vehicle_width = request->vehicle_width; - float vehicle_height = request->vehicle_height; - float vehicle_wheelbase = request->vehicle_wheelbase; - - if (vehicle_length < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_length); - vehicle_length = sensor_configuration_.configuration_vehicle_length; - } - - if (vehicle_width < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_width); - vehicle_width = sensor_configuration_.configuration_vehicle_width; - } - - if (vehicle_height < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_height); - vehicle_height = sensor_configuration_.configuration_vehicle_height; - } - - if (vehicle_wheelbase < 0.f) { - RCLCPP_INFO( - this->get_logger(), - "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", - sensor_configuration_.configuration_vehicle_wheelbase); - vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; - } - - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetVehicleParameters( - vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -void ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( - const std::shared_ptr - request, - const std::shared_ptr - response) -{ - std::scoped_lock lock(mtx_config_); - auto result = hw_interface_.SetRadarParameters( - request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, - request->country_code, request->powersave_standstill); - - response->success = result == Status::OK; - response->message = (std::stringstream() << result).str(); -} - -std::vector -ContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), - rclcpp::Parameter( - "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), - rclcpp::Parameter( - "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), - rclcpp::Parameter( - "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), - rclcpp::Parameter( - "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp new file mode 100644 index 000000000..17e5b5ca8 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -0,0 +1,287 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config_ptr) +: parent_node_(parent_node), + hw_interface_( + std::make_shared()), + logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), + status_(Status::NOT_INITIALIZED), + config_ptr_(config_ptr) +{ + hw_interface_->SetLogger( + std::make_shared(parent_node->get_logger().get_child("HwInterface"))); + status_ = hw_interface_->SetSensorConfiguration(config_ptr); + + if (status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); + } + + status_ = Status::OK; +} + +void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart() +{ + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_ = + parent_node_->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = + parent_node_->create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); + + steering_angle_sub_ = parent_node_->create_subscription( + "steering_angle_input", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + + set_network_configuration_service_server_ = + parent_node_->create_service( + "set_network_configuration", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + parent_node_->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + parent_node_->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + parent_node_->create_service( + "set_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } +} + +void ContinentalARS548HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & + new_config_ptr_ptr) +{ + hw_interface_->SetSensorConfiguration(new_config_ptr_ptr); + config_ptr_ = new_config_ptr_ptr; +} + +Status ContinentalARS548HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalARS548HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalARS548HwInterfaceWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_->SetDrivingDirection(0); + } else { + hw_interface_->SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_->SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void ContinentalARS548HwInterfaceWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + hw_interface_->SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_->SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +void ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) +{ + auto tf_buffer = std::make_unique(parent_node_->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + logger_, + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + config_ptr_->base_frame, config_ptr_->frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, "Could not obtain the transform from the base frame to %s (%s)", + config_ptr_->frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = + base_to_sensor_tf.transform.translation.x - config_ptr_->configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; + } + + auto result = hw_interface_->SetSensorMounting( + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) +{ + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_length); + vehicle_length = config_ptr_->configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_width); + vehicle_width = config_ptr_->configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_height); + vehicle_height = config_ptr_->configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + logger_, "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + config_ptr_->configuration_vehicle_wheelbase); + vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; + } + + auto result = hw_interface_->SetVehicleParameters( + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + auto result = hw_interface_->SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp new file mode 100644 index 000000000..6370c89c7 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -0,0 +1,352 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_ars548_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + wrapper_status_(Status::NOT_INITIALIZED), + packet_queue_(3000), + hw_interface_wrapper_(), + decoder_wrapper_() +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + wrapper_status_ = DeclareAndGetSensorConfigParams(); + + if (wrapper_status_ != Status::OK) { + throw std::runtime_error( + (std::stringstream{} << "Sensor configuration invalid: " << wrapper_status_).str()); + } + + RCLCPP_INFO_STREAM(get_logger(), "SensorConfig:" << *config_ptr_); + + launch_hw_ = declare_parameter("launch_hw", param_read_only()); + + if (launch_hw_) { + hw_interface_wrapper_.emplace(this, config_ptr_); + } + + decoder_wrapper_.emplace(this, config_ptr_, launch_hw_); + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterCallback( + std::bind(&ContinentalARS548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalARS548RosWrapper::ReceivePacketsMessageCallback, this, std::placeholders::_1)); + RCLCPP_INFO_STREAM( + get_logger(), + "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); + } + + // Register parameter callback after all params have been declared. Otherwise it would be called + // once for each declaration + parameter_event_cb_ = add_on_set_parameters_callback( + std::bind(&ContinentalARS548RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration config; + + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model"); + config.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + config.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + config.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + config.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + config.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + config.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + config.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + config.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + config.configuration_host_port = this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + config.configuration_sensor_port = this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + config.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + config.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + config.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + config.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_write(); + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + config.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(config); + return ValidateAndSetConfig(new_config_ptr); +} + +Status ContinentalARS548RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config_ptr) +{ + if (new_config_ptr->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config_ptr->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config_ptr); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config_ptr); + } + + config_ptr_ = new_config_ptr; + return Status::OK; +} + +void ContinentalARS548RosWrapper::ReceivePacketsMessageCallback( + std::unique_ptr packets_msg) +{ + if (hw_interface_wrapper_) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 1000, + "Ignoring NebulaPackets. Launch with launch_hw:=false to enable NebulaPackets " + "replay."); + return; + } + + for (auto & packet : packets_msg->packets) { + auto nebula_packet_ptr = std::make_unique(); + nebula_packet_ptr->stamp = packet.stamp; + std::copy(packet.data.begin(), packet.data.end(), std::back_inserter(nebula_packet_ptr->data)); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +Status ContinentalARS548RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalARS548RosWrapper::StreamStart() +{ + if (!hw_interface_wrapper_) { + return Status::UDP_CONNECTION_ERROR; + } + + if (hw_interface_wrapper_->Status() != Status::OK) { + return hw_interface_wrapper_->Status(); + } + + hw_interface_wrapper_->SensorInterfaceStart(); + + return hw_interface_wrapper_->Status(); +} + +rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + std::scoped_lock lock(mtx_config_); + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_ars548::ContinentalARS548SensorConfiguration new_config(*config_ptr_); + + bool got_any = + get_param(p, "frame_id", new_config.frame_id) | + get_param(p, "base_frame", new_config.base_frame) | + get_param(p, "object_frame", new_config.object_frame) | + get_param(p, "configuration_vehicle_length", new_config.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_config.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_config.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_config.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_config.configuration_sensor_port); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(new_config); + auto status = ValidateAndSetConfig(new_config_ptr); + + if (status != Status::OK) { + RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); + auto result = SetParametersResult(); + result.successful = false; + result.reason = (std::stringstream() << "Invalid configuration: " << status).str(); + return result; + } + + return rcl_interfaces::build().successful(true).reason(""); +} + +void ContinentalARS548RosWrapper::ReceivePacketCallback( + std::unique_ptr msg_ptr) +{ + if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + return; + } + + if (!packet_queue_.try_push(std::move(msg_ptr))) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp deleted file mode 100644 index b8af00fd2..000000000 --- a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ /dev/null @@ -1,356 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include - -#include -#include - -namespace nebula -{ -namespace ros -{ -MultiContinentalARS548HwInterfaceRosWrapper::MultiContinentalARS548HwInterfaceRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() -{ - if (mtx_config_.try_lock()) { - interface_status_ = GetParameters(sensor_configuration_); - mtx_config_.unlock(); - } - if (Status::OK != interface_status_) { - RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); - return; - } - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr - sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); - hw_interface_.RegisterScanCallback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); - sensor_id++) { - const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; - const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; - packets_pub_map_[sensor_ip] = this->create_publisher( - frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); - } - - set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - - StreamStart(); -} - -MultiContinentalARS548HwInterfaceRosWrapper::~MultiContinentalARS548HwInterfaceRosWrapper() -{ -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStart() -{ - if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.SensorInterfaceStart(); - } - - if (Status::OK == interface_status_) { - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, - std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, - std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, - std::bind( - &MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, - std::placeholders::_1)); - } - - return interface_status_; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status MultiContinentalARS548HwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed - const drivers::SensorConfigurationBase & sensor_configuration) -{ - std::stringstream ss; - ss << sensor_configuration; - RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); - return Status::OK; -} - -Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", descriptor); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("sensor_ips", descriptor); - sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", descriptor); - sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter>("frame_ids", descriptor); - sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", descriptor); - sensor_configuration.configuration_host_port = - this->get_parameter("configuration_host_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", descriptor); - sensor_configuration.configuration_sensor_port = - this->get_parameter("configuration_sensor_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("use_sensor_time", descriptor); - sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( - std::unique_ptr scan_buffer, const std::string & sensor_ip) -{ - packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); -} - -rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWrapper::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); - RCLCPP_DEBUG_STREAM(this->get_logger(), p); - RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); - RCLCPP_INFO_STREAM(this->get_logger(), p); - - drivers::continental_ars548::MultiContinentalARS548SensorConfiguration new_param{ - sensor_configuration_}; - RCLCPP_INFO_STREAM(this->get_logger(), new_param); - std::string sensor_model_str; - - if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | - get_param(p, "sensor_ips", new_param.sensor_ips) | - get_param(p, "frame_ids", new_param.frame_ids) | - get_param(p, "data_port", new_param.data_port) | - get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "base_frame", new_param.base_frame) | - get_param(p, "object_frame", new_param.object_frame) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "configuration_host_port", new_param.configuration_host_port) | - get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { - if (0 < sensor_model_str.length()) - new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); - - sensor_configuration_ = new_param; - RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); - std::shared_ptr sensor_cfg_ptr = - std::make_shared( - sensor_configuration_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - hw_interface_.CheckAndSetConfig(); - } - - auto result = std::make_shared(); - result->successful = true; - result->reason = "success"; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); - - return *result; -} - -void MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback( - const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - - constexpr float speed_to_standstill = 0.5f; - constexpr float speed_to_moving = 2.f; - - if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { - standstill_ = false; - } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { - standstill_ = true; - } - - if (standstill_) { - hw_interface_.SetDrivingDirection(0); - } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); - } - - constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); - - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( - const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); -} - -void MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( - const std_msgs::msg::Float32::SharedPtr msg) -{ - std::scoped_lock lock(mtx_config_); - constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); -} - -std::vector -MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() -{ - std::scoped_lock lock(mtx_config_); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); - std::ostringstream os_sensor_model; - os_sensor_model << sensor_configuration_.sensor_model; - RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); - auto results = set_parameters( - {rclcpp::Parameter("sensor_model", os_sensor_model.str()), - rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), - rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), - rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), - rclcpp::Parameter("data_port", sensor_configuration_.data_port), - rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), - rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), - rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), - rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); - RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); - return results; -} - -RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalARS548HwInterfaceRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index d31398c5b..1bd913f62 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -30,6 +30,7 @@ #include #include #include +#include namespace nebula { @@ -278,8 +279,9 @@ void ContinentalRosDecoderTest::ReadBag() ASSERT_EQ(1, extracted_msg.packets.size()); - auto extracted_msg_ptr = std::make_shared(extracted_msg); - driver_ptr_->ProcessPackets(extracted_msg); + auto extracted_msg_ptr = + std::make_unique(extracted_msg.packets[0]); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); } } } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index aab813cb2..f36cac677 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index a3203da89..9935e446f 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License.