From 204eb456081bf0091d465a206c02623b6f08f1bb Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 13 Jun 2024 20:41:58 +0900 Subject: [PATCH] feat(srr520): initial driver (#124) * feat: refactored the ars548 into the new single node scheme Signed-off-by: Kenzo Lobos-Tsunekawa * feat: mid implmementation of the single node scheme Signed-off-by: Kenzo Lobos-Tsunekawa * chore: cleaned code and removed unused code/includes Signed-off-by: Kenzo Lobos-Tsunekawa * chore: finished the refactoring. old bags will become unusable after this. need to test with actual hardware Signed-off-by: Kenzo Lobos-Tsunekawa * chore: deleted all nebula ros files Signed-off-by: Kenzo Lobos-Tsunekawa * chote: rebased and fixed rebse-related errors Signed-off-by: Kenzo Lobos-Tsunekawa * chore: forgot make changes Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed coyright Signed-off-by: Kenzo Lobos-Tsunekawa * chore: replaced Srr520 for SRR520 in class names and deleted unused files Signed-off-by: Kenzo Lobos-Tsunekawa * fix: fixed srr520 changes after the latest rebased and confirmed basic driver functionalities with a live sensor Signed-off-by: Kenzo Lobos-Tsunekawa * feat: brought back the srr520 tests (passing) Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed chunks of code that got in during the rebase. updated the config file Signed-off-by: Kenzo Lobos-Tsunekawa * chore: kept back a package that got upgraded automatically by pre commit Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added some comments regarding filters Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed segmentation when launch_hw was true and there was no can device. Changed the name of the config file for schema purposes Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated cspell Signed-off-by: Kenzo Lobos-Tsunekawa * chore: changes for clang compatibility Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added an explanation to the concept of time domain id Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_ros/config/radar/continental/SRR520.param.yaml Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> * Update nebula_common/include/nebula_common/nebula_common.hpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> * chore: added temptative schema Signed-off-by: Kenzo Lobos-Tsunekawa * chore: SRR520 schema still referenced the ARS548 radar Signed-off-by: Kenzo Lobos-Tsunekawa * chore: typo in the base schema Signed-off-by: Kenzo Lobos-Tsunekawa * chore: forgot the bus time in the schema Signed-off-by: Kenzo Lobos-Tsunekawa * chore: deleted unused methods Signed-off-by: Kenzo Lobos-Tsunekawa * chore: changed variable name from vambig -> v_ambiguous Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_common/include/nebula_common/continental/continental_srr520.hpp Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> * Update nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> * chore: changed magic numbers for constexprs Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed pandar mention in continental packages and changed callback name to be consistent among continental sensors Signed-off-by: Kenzo Lobos-Tsunekawa * chore: capitalized acronyms Signed-off-by: Kenzo Lobos-Tsunekawa * chore: changed fup for follow up Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_common/include/nebula_common/continental/crc.hpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> * chore: removed unnecessary moves Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated parameter loading scheme Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated srr test includes to match google code style Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added uniform initialization in srr tests Signed-off-by: Kenzo Lobos-Tsunekawa * chore: missing underscore in class variables for srr tests Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated includes in ARS tests to keep consistency Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated missing ars548 tests Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_ros/schema/sub/topic.json Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> * chore: updated to the new parameter loading scheme Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated include rules for follow nebula/autoware rules Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated missing include rules violations Signed-off-by: Kenzo Lobos-Tsunekawa * chore: missing include violations in the continental decoders Signed-off-by: Kenzo Lobos-Tsunekawa * chore: missing include violations in the continental hw interfaces Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added missing include Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com> --- .cspell.json | 2 + build_depends.repos | 2 +- .../continental/continental_ars548.hpp | 4 +- .../continental/continental_srr520.hpp | 296 +++ .../include/nebula_common/continental/crc.hpp | 68 + .../include/nebula_common/nebula_common.hpp | 35 +- .../include/nebula_common/nebula_status.hpp | 15 + nebula_decoders/CMakeLists.txt | 1 + .../decoders/continental_ars548_decoder.hpp | 3 +- .../decoders/continental_packets_decoder.hpp | 1 + .../decoders/continental_srr520_decoder.hpp | 211 ++ .../decoders/continental_ars548_decoder.cpp | 3 +- .../decoders/continental_srr520_decoder.cpp | 1210 ++++++++++ nebula_hw_interfaces/CMakeLists.txt | 6 +- .../continental_ars548_hw_interface.hpp | 9 +- .../continental_srr520_hw_interface.hpp | 148 ++ nebula_hw_interfaces/package.xml | 1 + .../continental_ars548_hw_interface.cpp | 5 +- .../continental_srr520_hw_interface.cpp | 405 ++++ .../continental_msgs/CMakeLists.txt | 4 + .../msg/ContinentalSrr520Detection.msg | 11 + .../msg/ContinentalSrr520DetectionList.msg | 10 + .../msg/ContinentalSrr520Object.msg | 21 + .../msg/ContinentalSrr520ObjectList.msg | 12 + .../continental_srvs/CMakeLists.txt | 1 + .../ContinentalSrr520SetRadarParameters.srv | 15 + nebula_ros/CMakeLists.txt | 43 + .../radar/continental/SRR520.param.yaml | 12 + .../continental_ars548_decoder_wrapper.hpp | 9 +- ...ontinental_ars548_hw_interface_wrapper.hpp | 3 +- .../continental_ars548_ros_wrapper.hpp | 13 +- .../continental_srr520_decoder_wrapper.hpp | 162 ++ ...ontinental_srr520_hw_interface_wrapper.hpp | 100 + .../continental_srr520_ros_wrapper.hpp | 101 + .../launch/continental_launch_all_hw.xml | 11 + nebula_ros/schema/SRR520.schema.json | 69 + nebula_ros/schema/sub/communication.json | 42 +- nebula_ros/schema/sub/radar_continental.json | 2 +- nebula_ros/schema/sub/topic.json | 6 + .../continental_ars548_decoder_wrapper.cpp | 9 +- ...ontinental_ars548_hw_interface_wrapper.cpp | 2 +- .../continental_ars548_ros_wrapper.cpp | 203 +- .../continental_srr520_decoder_wrapper.cpp | 603 +++++ ...ontinental_srr520_hw_interface_wrapper.cpp | 183 ++ .../continental_srr520_ros_wrapper.cpp | 218 ++ nebula_tests/continental/CMakeLists.txt | 27 + .../continental_ros_decoder_test_ars548.cpp | 136 +- .../continental_ros_decoder_test_ars548.hpp | 17 +- ...ntinental_ros_decoder_test_main_srr520.cpp | 60 + .../continental_ros_decoder_test_srr520.cpp | 259 ++ .../continental_ros_decoder_test_srr520.hpp | 96 + .../continental/parameter_descriptors.cpp | 34 + .../continental/parameter_descriptors.hpp | 44 + .../srr520/1708578208_992410284_object.yaml | 854 +++++++ .../srr520/1708578209/1708578209.db3 | Bin 0 -> 28672 bytes .../srr520/1708578209/metadata.yaml | 26 + .../1708578209_45566935_near_detection.yaml | 2147 +++++++++++++++++ .../1708578209_76111685_hrr_detection.yaml | 365 +++ 58 files changed, 8040 insertions(+), 315 deletions(-) create mode 100644 nebula_common/include/nebula_common/continental/continental_srr520.hpp create mode 100644 nebula_common/include/nebula_common/continental/crc.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp create mode 100644 nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp create mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg create mode 100644 nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv create mode 100644 nebula_ros/config/radar/continental/SRR520.param.yaml create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp create mode 100644 nebula_ros/schema/SRR520.schema.json create mode 100644 nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_srr520.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_srr520.hpp create mode 100644 nebula_tests/continental/parameter_descriptors.cpp create mode 100644 nebula_tests/continental/parameter_descriptors.hpp create mode 100644 nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209/1708578209.db3 create mode 100644 nebula_tests/data/continental/srr520/1708578209/metadata.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209_45566935_near_detection.yaml create mode 100644 nebula_tests/data/continental/srr520/1708578209_76111685_hrr_detection.yaml diff --git a/.cspell.json b/.cspell.json index 5b977fd6c..e83fb6742 100644 --- a/.cspell.json +++ b/.cspell.json @@ -27,6 +27,7 @@ "Msop", "nohup", "nproc", + "nsec", "ntoa", "pandar", "PANDAR", @@ -45,6 +46,7 @@ "struct", "structs", "UDP_SEQ", + "usec", "vccint", "Vccint", "Vdat", diff --git a/build_depends.repos b/build_depends.repos index 6b7e58ff7..8e7351482 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -8,4 +8,4 @@ repositories: ros2_socketcan: type: git url: https://github.com/knzo25/ros2_socketcan - version: feat/continental_fd + version: feat/continental_fd \ No newline at end of file diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 949b025be..06bdb3789 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -14,8 +14,8 @@ #pragma once -#include -#include +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" #include #include diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp new file mode 100644 index 000000000..ca75e6944 --- /dev/null +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -0,0 +1,296 @@ +// 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 "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ + +/// @brief struct for SRR520 sensor configuration +struct ContinentalSRR520SensorConfiguration : CANSensorConfigurationBase +{ + std::string base_frame{}; + bool sync_use_bus_time{}; + float configuration_vehicle_wheelbase{}; +}; + +/// @brief Convert ContinentalSRR520SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, ContinentalSRR520SensorConfiguration const & arg) +{ + os << (CANSensorConfigurationBase)(arg) << ", BaseFrame: " << arg.base_frame + << ", SyncUseBusTime: " << arg.sync_use_bus_time + << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; + return os; +} + +using boost::endian::big_float32_buf_t; +using boost::endian::big_uint16_buf_t; +using boost::endian::big_uint32_buf_t; +using boost::endian::big_uint64_buf_t; + +// CAN MSG IDS +constexpr int RDI_NEAR_HEADER_CAN_MESSAGE_ID = 900; +constexpr int RDI_NEAR_ELEMENT_CAN_MESSAGE_ID = 901; +constexpr int RDI_HRR_HEADER_CAN_MESSAGE_ID = 1100; +constexpr int RDI_HRR_ELEMENT_CAN_MESSAGE_ID = 1101; +constexpr int OBJECT_HEADER_CAN_MESSAGE_ID = 1200; +constexpr int OBJECT_CAN_MESSAGE_ID = 1201; +constexpr int CRC_LIST_CAN_MESSAGE_ID = 800; +constexpr int STATUS_CAN_MESSAGE_ID = 700; +constexpr int SYNC_FOLLOW_UP_CAN_MESSAGE_ID = 53; +constexpr int VEH_DYN_CAN_MESSAGE_ID = 600; +constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; + +// CRC IDS +constexpr int NEAR_CRC_ID = 0; +constexpr int HRR_CRC_ID = 1; +constexpr int OBJECT_CRC_ID = 2; +constexpr int TIME_DOMAIN_ID = + 0; // For details, please refer to AUTOSAR's "Time Synchronization over CAN" document + +constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; +constexpr int RDI_HRR_HEADER_PACKET_SIZE = 32; +constexpr int RDI_HRR_ELEMENT_PACKET_SIZE = 64; +constexpr int OBJECT_HEADER_PACKET_SIZE = 32; +constexpr int OBJECT_PACKET_SIZE = 64; +constexpr int CRC_LIST_PACKET_SIZE = 4; +constexpr int STATUS_PACKET_SIZE = 64; +constexpr int SYNC_FOLLOW_UP_CAN_PACKET_SIZE = 8; +constexpr int VEH_DYN_CAN_PACKET_SIZE = 8; +constexpr int CONFIGURATION_PACKET_SIZE = 16; + +constexpr int RDI_NEAR_PACKET_NUM = 50; +constexpr int RDI_HRR_PACKET_NUM = 20; +constexpr int OBJECT_PACKET_NUM = 20; + +constexpr int FRAGMENTS_PER_DETECTION_PACKET = 10; +constexpr int FRAGMENTS_PER_OBJECT_PACKET = 2; +constexpr int DETECTION_FRAGMENT_SIZE = 6; +constexpr int OBJECT_FRAGMENT_SIZE = 31; + +constexpr int MAX_RDI_NEAR_DETECTIONS = RDI_NEAR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_RDI_HRR_DETECTIONS = RDI_HRR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; +constexpr int MAX_OBJECTS = OBJECT_PACKET_NUM * FRAGMENTS_PER_OBJECT_PACKET; + +#pragma pack(push, 1) + +struct StatusPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_sw_version_major; // 13 + uint8_t u_sw_version_minor; // 14 + uint8_t u_sw_version_patch; // 15 + uint8_t u_sensor_id; // 16 + big_uint16_buf_t u_long_pos; // 17 + big_uint16_buf_t u_lat_pos; // 19 + big_uint16_buf_t u_vert_pos; // 21 + big_uint16_buf_t u_long_pos_cog; // 23 + big_uint16_buf_t u_wheelbase; // 25 + big_uint16_buf_t u_yaw_angle; // 27 + big_uint16_buf_t u_cover_damping; // 29 + uint8_t u_plug_orientation; // 31 + uint8_t u_defective; // 32 + uint8_t u_supply_voltage_limit; // 33 + uint8_t u_sensor_off_temp; // 34 + uint8_t u_dynamics_invalid0; // 35 + uint8_t u_dynamics_invalid1; // 36 + uint8_t u_ext_disturbed; // 37 + uint8_t u_com_error; // 38 + big_uint16_buf_t u_sw_error; // 39 + uint8_t u_aln_status_azimuth_available; // 41 + big_uint16_buf_t u_aln_current_azimuth_std; // 42 + big_uint16_buf_t u_aln_current_azimuth; // 44 + big_uint16_buf_t u_aln_current_delta; // 46 + uint8_t reserved1[13]; // Reserved fields, no change needed + big_uint16_buf_t u_crc; // 61 + uint8_t u_sequence_counter; // 63 +}; + +struct ScanHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_v_ambiguous; // 19 + big_uint16_buf_t u_max_range; // 21 + big_uint16_buf_t u_number_of_detections; // 23 + uint8_t reserved[7]; // 25 +}; + +struct DetectionFragmentPacket +{ + uint8_t data[DETECTION_FRAGMENT_SIZE]; +}; + +struct DetectionPacket +{ + DetectionFragmentPacket fragments[FRAGMENTS_PER_DETECTION_PACKET]; // 0 - 59 + uint8_t reserved0; // 60 + uint8_t reserved1; // 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ObjectHeaderPacket +{ + big_uint32_buf_t u_time_stamp; // 0 + big_uint32_buf_t u_global_time_stamp_sec; // 4 + big_uint32_buf_t u_global_time_stamp_nsec; // 8 + uint8_t u_global_time_stamp_sync_status; // 12 + uint8_t u_signal_status; // 13 + uint8_t u_sequence_counter; // 14 + big_uint32_buf_t u_cycle_counter; // 15 + big_uint16_buf_t u_ego_vx; // 19 + big_uint16_buf_t u_ego_yaw_rate; // 21 + uint8_t u_motion_type; // 23 + uint8_t u_number_of_objects; // 24 + uint8_t reserved[7]; // 25 +}; + +struct ObjectFragmentPacket +{ + uint8_t data[OBJECT_FRAGMENT_SIZE]; +}; + +struct ObjectPacket +{ + ObjectFragmentPacket fragments[FRAGMENTS_PER_OBJECT_PACKET]; // 0 - 61 + uint8_t u_message_counter; // 62 + uint8_t u_sequence_counter; // 63 +}; + +struct ConfigurationPacket +{ + uint8_t u_sensor_id; + big_uint16_buf_t u_long_pose; + big_uint16_buf_t u_lat_pose; + big_uint16_buf_t u_vert_pos; + big_uint16_buf_t u_long_pos_cog; + big_uint16_buf_t u_wheelbase; + big_uint16_buf_t u_yaw_angle; + big_uint16_buf_t u_cover_damping; + uint8_t u_plug_orientation_and_default_settings; +}; + +struct SyncPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_user_data0; + big_uint32_buf_t u_sync_time_sec; +}; + +struct FollowUpPacket +{ + uint8_t u_type; + uint8_t u_crc; + uint8_t u_time_domain_and_sequence_counter; + uint8_t u_reserved; + big_uint32_buf_t u_sync_time_nsec; +}; + +#pragma pack(pop) + +struct EIGEN_ALIGN16 PointSRR520Detection +{ + PCL_ADD_POINT4D; + float range; + float azimuth; + float range_rate; + float rcs; + uint8_t pdh00; + uint8_t pdh01; + uint8_t pdh02; + uint8_t pdh03; + uint8_t pdh04; + uint8_t pdh05; + float snr; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +// Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the +// number of fields +struct EIGEN_ALIGN16 PointSRR520Object +{ + PCL_ADD_POINT4D; + uint32_t id; + uint16_t age; + float orientation; + float rcs; + float score; + uint8_t object_status; + float dynamics_abs_vel_x; + float dynamics_abs_vel_y; + float dynamics_abs_acc_x; + float dynamics_abs_acc_y; + float box_length; + float box_width; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSRR520Detection, + (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, range, range)( + float, range_rate, range_rate)(int8_t, rcs, rcs)(uint8_t, pdh00, pdh00)(uint8_t, pdh01, pdh01)( + uint8_t, pdh02, + pdh02)(uint16_t, pdh03, pdh03)(uint8_t, pdh04, pdh04)(uint8_t, pdh05, pdh05)(float, snr, snr)) + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_srr520::PointSRR520Object, + (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( + float, orientation, + orientation)(float, rcs, rcs)(float, score, score)(uint8_t, object_status, object_status)( + float, dynamics_abs_vel_x, dynamics_abs_vel_x)(float, dynamics_abs_vel_y, dynamics_abs_vel_y)( + float, dynamics_abs_acc_x, dynamics_abs_acc_x)(float, dynamics_abs_acc_y, dynamics_abs_acc_y)( + float, box_length, box_length)(float, box_width, box_width)) diff --git a/nebula_common/include/nebula_common/continental/crc.hpp b/nebula_common/include/nebula_common/continental/crc.hpp new file mode 100644 index 000000000..733f5d0fd --- /dev/null +++ b/nebula_common/include/nebula_common/continental/crc.hpp @@ -0,0 +1,68 @@ +// 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 + +template +int crc16_packets(Iterator begin, Iterator end, int payload_offset) +{ + uint16_t crc_word = 0xffff; + + for (Iterator it = begin; it != end; ++it) { + for (auto it2 = it->data.begin() + payload_offset; it2 != it->data.end(); ++it2) { + auto byte = *it2; + crc_word ^= byte << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + } + + return crc_word; +} + +template +int crc16_packet(Iterator begin, Iterator end) +{ + uint16_t crc_word = 0xffff; + for (Iterator it = begin; it != end; ++it) { + crc_word ^= (*it) << 8; + for (int i = 0; i < 8; i++) + crc_word = crc_word & 0x8000 ? (crc_word << 1) ^ 0x1021 : crc_word << 1; + } + + return crc_word; +} + +template +uint8_t crc8h2f(Iterator begin, Iterator end) +{ + uint8_t crc_word = 0xFF; + uint8_t bit = 0; + + for (Iterator it = begin; it != end; ++it) { + crc_word ^= *it; + for (bit = 0; bit < 8; bit++) { + if ((crc_word & 0x80) != 0) { + crc_word <<= 1; + crc_word ^= 0x2F; + } else { + crc_word <<= 1; + } + } + } + + return crc_word ^ 0xFF; +} diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index a7deeae7c..0331179c0 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -329,7 +329,8 @@ enum class SensorModel { ROBOSENSE_HELIOS, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, - CONTINENTAL_ARS548 + CONTINENTAL_ARS548, + CONTINENTAL_SRR520 }; /// @brief not used? @@ -426,6 +427,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::CONTINENTAL_ARS548: os << "ARS548"; break; + case SensorModel::CONTINENTAL_SRR520: + os << "SRR520"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -448,6 +452,17 @@ struct EthernetSensorConfigurationBase : SensorConfigurationBase uint16_t data_port; }; +/// @brief Base struct for CAN-based Sensor configuration +struct CANSensorConfigurationBase : SensorConfigurationBase +{ + std::string interface; + float receiver_timeout_sec{}; + float sender_timeout_sec{}; + /// @brief Socketcan filters, see the documentation of SocketCanReceiver::CanFilterList for details + std::string filters{}; + bool use_bus_time{}; +}; + /// @brief Base struct for Lidar configuration struct LidarConfigurationBase : EthernetSensorConfigurationBase { @@ -483,6 +498,19 @@ inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationB return os; } +/// @brief Convert CANSensorConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, CANSensorConfigurationBase const & arg) +{ + os << (SensorConfigurationBase)(arg) + << ", Interface: " << arg.interface << ", ReceiverTimeoutSec: " << arg.receiver_timeout_sec + << ", SenderTimeoutSec: " << arg.sender_timeout_sec << ", Filters: " << arg.filters + << ", UseBusTime: " << arg.use_bus_time; + return os; +} + /// @brief Convert LidarConfigurationBase to string (Overloading the << operator) /// @param os /// @param arg @@ -529,7 +557,9 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "Bpearl" || sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; + // Continental if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; + if (sensor_model == "SRR520") return SensorModel::CONTINENTAL_SRR520; return SensorModel::UNKNOWN; } @@ -575,8 +605,11 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + // Continental case SensorModel::CONTINENTAL_ARS548: return "ARS548"; + case SensorModel::CONTINENTAL_SRR520: + return "SRR520"; default: return "UNKNOWN"; } diff --git a/nebula_common/include/nebula_common/nebula_status.hpp b/nebula_common/include/nebula_common/nebula_status.hpp index f36d8b7d2..7d04fae55 100644 --- a/nebula_common/include/nebula_common/nebula_status.hpp +++ b/nebula_common/include/nebula_common/nebula_status.hpp @@ -1,3 +1,17 @@ +// 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_STATUS_HPP #define NEBULA_STATUS_HPP @@ -14,6 +28,7 @@ struct Status enum Type { OK = 0, UDP_CONNECTION_ERROR, + CAN_CONNECTION_ERROR, SENSOR_CONFIG_ERROR, INVALID_SENSOR_MODEL, INVALID_ECHO_MODE, diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 7238e0d67..e94afcf2a 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -98,6 +98,7 @@ target_include_directories(nebula_decoders_robosense_info PUBLIC # Continental add_library(nebula_decoders_continental SHARED src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp + src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp ) target_link_libraries(nebula_decoders_continental PUBLIC ${continental_msgs_TARGETS} 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 af0c508e3..b017e6efd 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 @@ -14,8 +14,9 @@ #pragma once +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + #include -#include #include #include 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 8760c11b0..933490542 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 @@ -15,6 +15,7 @@ #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP +#include #include #include diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp new file mode 100644 index 000000000..c70cb4706 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -0,0 +1,211 @@ +// 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 "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Continental Radar decoder (SRR520) +class ContinentalSRR520Decoder : public ContinentalPacketsDecoder +{ +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + explicit ContinentalSRR520Decoder( + 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 ProcessPacket(std::unique_ptr packet_msg) override; + + /// @brief Register function to call whenever a new RDI near detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterNearDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new RDI HRR detection list is processed + /// @param detection_list_callback + /// @return Resulting status + Status RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterObjectListCallback( + std::function)> + object_list_callback); + + /// @brief Register function to call whenever a new object list is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterStatusCallback( + std::function)> status_callback); + + /// @brief Register function to call whenever a sync follow-up packet is processed + /// @param sync_follow_up_callback + /// @return Resulting status + Status RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback); + + /// @brief Register function to call whenever enough packets have been processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterPacketsCallback( + std::function)> nebula_packets_callback); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); + +private: + /// @brief Process a new near detection header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new near element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new hrr element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object header packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectHeaderPacket(std::unique_ptr packet_msg); + + /// @brief Process a new object element packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectElementPacket(std::unique_ptr packet_msg); + + /// @brief Process a new crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new Near detections crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessNearCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new HRR crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessHRRCRCListPacket( + std::unique_ptr packet_msg); // cspell:ignore HRRCRC + + /// @brief Process a new objects crc list packet + /// @param buffer The buffer containing the packet + /// @param stamp The stamp in nanoseconds + void ProcessObjectCRCListPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSensorStatusPacket(std::unique_ptr packet_msg); + + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet + /// @param stamp The stamp in nanoseconds + void ProcessSyncFollowUpPacket(std::unique_ptr packet_msg); + + /// @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); + + std::function msg)> + near_detection_list_callback_{}; + std::function msg)> + hrr_detection_list_callback_{}; + std::function msg)> + object_list_callback_{}; + std::function msg)> + status_callback_{}; + std::function sync_follow_up_callback_{}; + std::function msg)> + nebula_packets_callback_{}; + + std::unique_ptr rdi_near_packets_ptr_{}; + std::unique_ptr rdi_hrr_packets_ptr_{}; + std::unique_ptr object_packets_ptr_{}; + + std::unique_ptr near_detection_list_ptr_{}; + std::unique_ptr hrr_detection_list_ptr_{}; + std::unique_ptr object_list_ptr_{}; + + bool first_rdi_near_packet_{true}; + bool first_rdi_hrr_packet_{true}; + bool first_object_packet_{true}; + + ScanHeaderPacket rdi_near_header_packet_{}; + ScanHeaderPacket rdi_hrr_header_packet_{}; + ObjectHeaderPacket object_header_packet_{}; + + /// @brief SensorConfiguration for this decoder + std::shared_ptr + sensor_configuration_{}; + + std::shared_ptr parent_node_logger_ptr_{}; +}; + +} // namespace continental_srr520 +} // 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 8423d4a0b..548867b7f 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 @@ -12,8 +12,9 @@ // 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 -#include #include #include diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp new file mode 100644 index 000000000..d559b5714 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -0,0 +1,1210 @@ +// 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 "nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp" + +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSRR520Decoder::ContinentalSRR520Decoder( + const std::shared_ptr & + sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); +} + +Status ContinentalSRR520Decoder::GetStatus() +{ + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterNearDetectionListCallback( + std::function)> + detection_list_callback) +{ + near_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterHRRDetectionListCallback( + std::function)> + detection_list_callback) +{ + hrr_detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterObjectListCallback( + std::function)> + object_list_callback) +{ + object_list_callback_ = std::move(object_list_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterStatusCallback( + std::function)> status_callback) +{ + status_callback_ = std::move(status_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterSyncFollowUpCallback( + std::function sync_follow_up_callback) +{ + sync_follow_up_callback_ = std::move(sync_follow_up_callback); + return Status::OK; +} + +Status ContinentalSRR520Decoder::RegisterPacketsCallback( + std::function)> nebula_packets_callback) +{ + nebula_packets_callback_ = std::move(nebula_packets_callback); + return Status::OK; +} + +bool ContinentalSRR520Decoder::ProcessPacket( + std::unique_ptr packet_msg) +{ + const uint32_t can_message_id = (static_cast(packet_msg->data[0]) << 24) | + (static_cast(packet_msg->data[1]) << 16) | + (static_cast(packet_msg->data[2]) << 8) | + static_cast(packet_msg->data[3]); + + std::size_t payload_size = packet_msg->data.size() - 4; + + if (can_message_id == RDI_NEAR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_HEADER_PACKET_SIZE) { + PrintError("RDI_NEAR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessNearHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_NEAR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_NEAR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_NEAR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessNearElementPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_HEADER_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_HEADER_PACKET_SIZE) { + PrintError("RDI_HRR_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessHRRHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == RDI_HRR_ELEMENT_CAN_MESSAGE_ID) { + if (payload_size != RDI_HRR_ELEMENT_PACKET_SIZE) { + PrintError("RDI_HRR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessHRRElementPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_HEADER_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_HEADER_PACKET_SIZE) { + PrintError("OBJECT_HEADER_CAN_MESSAGE_ID message with invalid size"); + return false; + } + ProcessObjectHeaderPacket(std::move(packet_msg)); + } else if (can_message_id == OBJECT_CAN_MESSAGE_ID) { + if (payload_size != OBJECT_PACKET_SIZE) { + PrintError("OBJECT_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessObjectElementPacket(std::move(packet_msg)); + } else if (can_message_id == CRC_LIST_CAN_MESSAGE_ID) { + if (payload_size != CRC_LIST_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessCRCListPacket(std::move(packet_msg)); + } else if (can_message_id == STATUS_CAN_MESSAGE_ID) { + if (payload_size != STATUS_PACKET_SIZE) { + PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSensorStatusPacket(std::move(packet_msg)); + } else if (can_message_id == SYNC_FOLLOW_UP_CAN_MESSAGE_ID) { + if (payload_size != SYNC_FOLLOW_UP_CAN_PACKET_SIZE) { + PrintError("SYNC_FOLLOW_UP_CAN_MESSAGE_ID message with invalid size"); + return false; + } + + ProcessSyncFollowUpPacket(std::move(packet_msg)); + } else if ( + can_message_id != VEH_DYN_CAN_MESSAGE_ID && can_message_id != SENSOR_CONFIG_CAN_MESSAGE_ID) { + PrintError("Unrecognized message ID=" + std::to_string(can_message_id)); + return false; + } + + return true; +} + +void ContinentalSRR520Decoder::ProcessNearHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + + first_rdi_near_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_near_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_near_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_near_header_packet_.u_global_time_stamp_sync_status <= 3); + + near_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_near_header_packet_.u_global_time_stamp_sync_status == 1) { + near_detection_list_ptr_->header.stamp.sec = + rdi_near_header_packet_.u_global_time_stamp_sec.value(); + near_detection_list_ptr_->header.stamp.nanosec = + rdi_near_header_packet_.u_global_time_stamp_nsec.value(); + } else { + near_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_near_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_near_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + near_detection_list_ptr_->internal_time_stamp_usec = rdi_near_header_packet_.u_time_stamp.value(); + near_detection_list_ptr_->global_time_stamp_sync_status = + rdi_near_header_packet_.u_global_time_stamp_sync_status; + near_detection_list_ptr_->signal_status = rdi_near_header_packet_.u_signal_status; + near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; + near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); + near_detection_list_ptr_->v_ambiguous = + V_AMBIGUOUS_RESOLUTION * rdi_near_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + near_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_near_header_packet_.u_max_range.value(); + + near_detection_list_ptr_->detections.reserve( + rdi_near_header_packet_.u_number_of_detections.value()); + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessNearElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + + if (rdi_near_packets_ptr_->packets.size() == 0) { + if (!first_rdi_near_packet_) { + PrintError("Near element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = near_detection_list_ptr_->detections.size(); + + if ( + near_detection_list_ptr_->detections.size() >= + rdi_near_header_packet_.u_number_of_detections.value()) { + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_NEAR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_near_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert( + rdi_near_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_near_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = RANGE_RESOLUTION * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; + + near_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; + constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; + constexpr float MAX_RANGE_RESOLUTION = 0.1f; + + first_rdi_hrr_packet_ = false; + + static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_HRR_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &rdi_hrr_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ScanHeaderPacket)); + + assert( + rdi_hrr_header_packet_.u_global_time_stamp_sync_status >= 1 && + rdi_hrr_header_packet_.u_global_time_stamp_sync_status <= 3); + + hrr_detection_list_ptr_->header.frame_id = sensor_configuration_->frame_id; + + if (rdi_hrr_header_packet_.u_global_time_stamp_sync_status == 1) { + hrr_detection_list_ptr_->header.stamp.sec = + rdi_hrr_header_packet_.u_global_time_stamp_sec.value(); + hrr_detection_list_ptr_->header.stamp.nanosec = + rdi_hrr_header_packet_.u_global_time_stamp_nsec.value(); + } else { + hrr_detection_list_ptr_->header.stamp = packet_msg->stamp; + } + + rdi_hrr_packets_ptr_->header.stamp = packet_msg->stamp; + rdi_hrr_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + hrr_detection_list_ptr_->internal_time_stamp_usec = rdi_hrr_header_packet_.u_time_stamp.value(); + hrr_detection_list_ptr_->global_time_stamp_sync_status = + rdi_hrr_header_packet_.u_global_time_stamp_sync_status; + hrr_detection_list_ptr_->signal_status = rdi_hrr_header_packet_.u_signal_status; + hrr_detection_list_ptr_->sequence_counter = rdi_hrr_header_packet_.u_sequence_counter; + hrr_detection_list_ptr_->cycle_counter = rdi_hrr_header_packet_.u_cycle_counter.value(); + hrr_detection_list_ptr_->v_ambiguous = + V_AMBIGUOUS_RESOLUTION * rdi_hrr_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + hrr_detection_list_ptr_->max_range = + MAX_RANGE_RESOLUTION * rdi_hrr_header_packet_.u_max_range.value(); + + hrr_detection_list_ptr_->detections.reserve( + rdi_hrr_header_packet_.u_number_of_detections.value()); + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessHRRElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto RANGE_RESOLUTION = 0.024420024; + constexpr auto AZIMUTH_RESOLUTION = 0.006159986; + constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; + constexpr auto RCS_RESOLUTION = 0.476190476; + constexpr auto SNR_RESOLUTION = 1.7f; + + constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; + constexpr auto RANGE_RATE_MIN_VALUE = -15.f; + constexpr auto RCS_MIN_VALUE = -40.f; + constexpr auto SNR_MIN_VALUE = 11.f; + + if (rdi_hrr_packets_ptr_->packets.size() == 0) { + if (!first_rdi_hrr_packet_) { + PrintError("HRR element before header. This can happen during the first iteration"); + } + return; + } + + int parsed_detections = hrr_detection_list_ptr_->detections.size(); + + if ( + hrr_detection_list_ptr_->detections.size() >= + rdi_hrr_header_packet_.u_number_of_detections.value()) { + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + DetectionPacket detection_packet; + std::memcpy( + &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); + + static_assert(sizeof(DetectionPacket) == RDI_HRR_ELEMENT_PACKET_SIZE); + assert(packet_msg->data.size() == RDI_HRR_ELEMENT_PACKET_SIZE + 4); + assert(rdi_hrr_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); + assert( + rdi_hrr_packets_ptr_->packets.size() == + static_cast(detection_packet.u_message_counter + 1)); + + for (const auto & fragment : detection_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Detection detection_msg; + const auto & data = fragment.data; + + if (parsed_detections >= rdi_hrr_header_packet_.u_number_of_detections.value()) { + break; + } + + uint16_t u_range = + (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); + assert(u_range <= 4095); + detection_msg.range = RANGE_RESOLUTION * u_range; + + uint16_t u_azimuth = + (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); + assert(u_azimuth <= 510); + detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; + + uint16_t u_range_rate = + (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); + assert(u_range_rate <= 2046); + detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; + + uint16_t u_rcs = (data[4] & 0xFE) >> 1; + assert(u_rcs <= 126); + detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; + + detection_msg.pdh00 = 100 * (data[4] & 0x01); + detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); + detection_msg.pdh02 = 100 * ((data[5] & 0x40) >> 6); + detection_msg.pdh03 = 100 * ((data[5] & 0x20) >> 5); + detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); + + uint8_t u_snr = data[5] & 0x0f; + detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; + + hrr_detection_list_ptr_->detections.push_back(detection_msg); + parsed_detections++; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( + std::unique_ptr packet_msg) +{ + constexpr auto VX_RESOLUTION = 0.003051851; + constexpr auto VX_MIN_VALUE = -100.f; + constexpr auto YAW_RATE_RESOLUTION = 9.58766e-05; + constexpr auto YAW_RATE_MIN_VALUE = -3.14159; + + first_object_packet_ = false; + + static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_HEADER_PACKET_SIZE + 4); + + std::memcpy( + &object_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), + sizeof(ObjectHeaderPacket)); + + assert( + object_header_packet_.u_global_time_stamp_sync_status >= 1 && + object_header_packet_.u_global_time_stamp_sync_status <= 3); + + object_list_ptr_->header.frame_id = sensor_configuration_->base_frame; + + if (object_header_packet_.u_global_time_stamp_sync_status == 1) { + object_list_ptr_->header.stamp.sec = object_header_packet_.u_global_time_stamp_sec.value(); + object_list_ptr_->header.stamp.nanosec = object_header_packet_.u_global_time_stamp_nsec.value(); + } else { + object_list_ptr_->header.stamp = packet_msg->stamp; + } + + object_packets_ptr_->header.stamp = packet_msg->stamp; + object_packets_ptr_->header.frame_id = sensor_configuration_->base_frame; + + object_list_ptr_->internal_time_stamp_usec = object_header_packet_.u_time_stamp.value(); + object_list_ptr_->global_time_stamp_sync_status = + object_header_packet_.u_global_time_stamp_sync_status; + object_list_ptr_->signal_status = object_header_packet_.u_signal_status; + object_list_ptr_->sequence_counter = object_header_packet_.u_sequence_counter; + object_list_ptr_->cycle_counter = object_header_packet_.u_cycle_counter.value(); + object_list_ptr_->ego_vx = VX_RESOLUTION * object_header_packet_.u_ego_vx.value() + VX_MIN_VALUE; + object_list_ptr_->ego_yaw_rate = + YAW_RATE_RESOLUTION * object_header_packet_.u_ego_yaw_rate.value() + YAW_RATE_MIN_VALUE; + object_list_ptr_->motion_type = object_header_packet_.u_motion_type; + + object_list_ptr_->objects.reserve(object_header_packet_.u_number_of_objects); + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessObjectElementPacket( + std::unique_ptr packet_msg) +{ + constexpr auto DIST_RESOLUTION = 0.009155553; + constexpr auto V_ABS_RESOLUTION = 0.009156391; + constexpr auto A_ABS_RESOLUTION = 0.019569472; + constexpr auto DIST_STD_RESOLUTION = 0.001831166; + constexpr auto V_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto A_ABS_STD_RESOLUTION = 0.001831166; + constexpr auto OBJECT_BOX_RESOLUTION = 0.007326007; + constexpr auto OBJECT_ORIENTATION_RESOLUTION = 0.001534729; + constexpr auto OBJECT_RCS_RESOLUTION = 0.024425989; + constexpr auto OBJECT_SCORE_RESOLUTION = 6.666666667; + + constexpr auto DIST_MIN_VALUE = -300.f; + constexpr auto V_ABS_MIN_VALUE = -75.f; + constexpr auto A_ABS_MIN_VALUE = -10.f; + constexpr auto OBJECT_ORIENTATION_MIN_VALUE = -3.14159; + constexpr auto OBJECT_RCS_MIN_VALUE = -50.f; + + if (object_packets_ptr_->packets.size() == 0) { + if (!first_object_packet_) { + PrintError("Object element before header. This can happen during the first iteration"); + } + return; + } + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + return; + } + + ObjectPacket object_packet; + std::memcpy(&object_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(ObjectPacket)); + + static_assert(sizeof(ObjectPacket) == OBJECT_PACKET_SIZE); + assert(packet_msg->data.size() == OBJECT_PACKET_SIZE + 4); + assert(object_header_packet_.u_sequence_counter == object_packet.u_sequence_counter); + assert( + object_packets_ptr_->packets.size() == + static_cast(object_packet.u_message_counter + 1)); + + for (const auto & fragment : object_packet.fragments) { + continental_msgs::msg::ContinentalSrr520Object object_msg; + const auto & data = fragment.data; + + if (object_list_ptr_->objects.size() >= object_header_packet_.u_number_of_objects) { + break; + } + + object_msg.object_id = data[0]; + + uint16_t u_dist_x = ((static_cast(data[1]) << 8) | data[2]); + uint16_t u_dist_y = ((static_cast(data[3]) << 8) | data[4]); + assert(u_dist_x <= 65534); + assert(u_dist_y <= 65534); + object_msg.dist_x = DIST_RESOLUTION * u_dist_x + DIST_MIN_VALUE; + object_msg.dist_y = DIST_RESOLUTION * u_dist_y + DIST_MIN_VALUE; + + uint16_t u_v_abs_x = + (static_cast(data[5]) << 6) | (static_cast(data[6] & 0xfc) >> 2); + assert(u_v_abs_x <= 16382); + object_msg.v_abs_x = V_ABS_RESOLUTION * u_v_abs_x + V_ABS_MIN_VALUE; + + uint16_t u_v_abs_y = (static_cast(data[6] & 0x03) << 12) | + (static_cast(data[7]) << 4) | + (static_cast(data[8] & 0xF0) >> 4); + assert(u_v_abs_y <= 16382); + object_msg.v_abs_y = V_ABS_RESOLUTION * u_v_abs_y + V_ABS_MIN_VALUE; + + uint16_t u_a_abs_x = + (static_cast(data[8] & 0x0f) << 6) | (static_cast(data[9] & 0xfc) >> 2); + assert(u_a_abs_x <= 1022); + object_msg.a_abs_x = A_ABS_RESOLUTION * u_a_abs_x + A_ABS_MIN_VALUE; + + uint16_t u_a_abs_y = + (static_cast(data[9] & 0x03) << 8) | static_cast(data[10]); + assert(u_a_abs_y <= 1022); + object_msg.a_abs_y = A_ABS_RESOLUTION * u_a_abs_y + A_ABS_MIN_VALUE; + + uint16_t u_dist_x_std = + (static_cast(data[11]) << 6) | (static_cast(data[12] & 0xfc) >> 2); + assert(u_dist_x_std <= 16383); + object_msg.dist_x_std = DIST_STD_RESOLUTION * u_dist_x_std; + + uint16_t u_dist_y_std = (static_cast(data[12] & 0x03) << 12) | + (static_cast(data[13]) << 4) | + (static_cast(data[14] & 0xF0) >> 4); + assert(u_dist_y_std <= 16383); + object_msg.dist_y_std = DIST_STD_RESOLUTION * u_dist_y_std; + + uint16_t u_v_abs_x_std = (static_cast(data[14] & 0x0f) << 10) | + (static_cast(data[15]) << 2) | + (static_cast(data[15] & 0x03) >> 6); + assert(u_v_abs_x_std <= 16383); + object_msg.v_abs_x_std = V_ABS_STD_RESOLUTION * u_v_abs_x_std; + + uint16_t u_v_abs_y_std = + (static_cast(data[16] & 0x3f) << 8) | static_cast(data[17]); + assert(u_v_abs_y_std <= 16383); + object_msg.v_abs_y_std = V_ABS_STD_RESOLUTION * u_v_abs_y_std; + + uint16_t u_a_abs_x_std = + (static_cast(data[18]) << 6) | (static_cast(data[19] & 0xfc) >> 2); + assert(u_a_abs_x_std <= 16383); + object_msg.a_abs_x_std = A_ABS_STD_RESOLUTION * u_a_abs_x_std; + + uint16_t u_a_abs_y_std = (static_cast(data[19] & 0x03) << 12) | + (static_cast(data[20]) << 4) | + (static_cast(data[21] & 0xF0) >> 4); + assert(u_a_abs_y_std <= 16383); + object_msg.a_abs_y_std = A_ABS_STD_RESOLUTION * u_a_abs_y_std; + + uint16_t u_box_length = + (static_cast(data[21] & 0x0f) << 8) | static_cast(data[22]); + assert(u_box_length <= 4095); + object_msg.box_length = OBJECT_BOX_RESOLUTION * u_box_length; + + uint16_t u_box_width = + (static_cast(data[23]) << 4) | (static_cast(data[24] & 0xF0) >> 4); + assert(u_box_width <= 4095); + object_msg.box_width = OBJECT_BOX_RESOLUTION * u_box_width; + + uint16_t u_orientation = + (static_cast(data[24] & 0x0f) << 8) | static_cast(data[25]); + assert(u_orientation <= 4094); + object_msg.orientation = + OBJECT_ORIENTATION_RESOLUTION * u_orientation + OBJECT_ORIENTATION_MIN_VALUE; + + uint16_t u_rcs = + (static_cast(data[26]) << 4) | (static_cast(data[27] & 0xF0) >> 4); + assert(u_rcs <= 4094); + object_msg.rcs = OBJECT_RCS_RESOLUTION * u_rcs + OBJECT_RCS_MIN_VALUE; + + uint8_t u_score = data[27] & 0x0f; + assert(u_score <= 15); + object_msg.score = OBJECT_SCORE_RESOLUTION * u_score; + + object_msg.life_cycles = (static_cast(data[28]) << 8) | data[29]; + assert(object_msg.life_cycles <= 65535); + + object_msg.box_valid = data[30] & 0x01; + object_msg.object_status = (data[30] & 0x06) >> 1; + + object_list_ptr_->objects.push_back(object_msg); + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); +} + +void ContinentalSRR520Decoder::ProcessCRCListPacket( + std::unique_ptr packet_msg) +{ + const auto crc_id = packet_msg->data[4]; // first 4 bits are the can id + + if (crc_id == NEAR_CRC_ID) { + ProcessNearCRCListPacket(std::move(packet_msg)); + } else if (crc_id == HRR_CRC_ID) { + ProcessHRRCRCListPacket(std::move(packet_msg)); // cspell: ignore HRRCRC + } else if (crc_id == OBJECT_CRC_ID) { + ProcessObjectCRCListPacket(std::move(packet_msg)); + } else { + PrintError(std::string("Unrecognized CRC id=") + std::to_string(crc_id)); + } +} + +void ContinentalSRR520Decoder::ProcessNearCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_near_packets_ptr_->packets.size() != RDI_NEAR_PACKET_NUM + 1) { + if (!first_rdi_near_packet_) { + PrintError("Incorrect number of RDI Near elements before CRC list"); + } + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_near_packets_ptr_->packets.begin(), rdi_near_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); + return; + } + + rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (near_detection_list_callback_) { + near_detection_list_callback_(std::move(near_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_near_packets_ptr_)); + } + + rdi_near_packets_ptr_ = std::make_unique(); + near_detection_list_ptr_ = + std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( + std::unique_ptr packet_msg) +{ + if (rdi_hrr_packets_ptr_->packets.size() != RDI_HRR_PACKET_NUM + 1) { + if (!first_rdi_hrr_packet_) { + PrintError("Incorrect number of RDI HRR elements before CRC list"); + } + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(rdi_hrr_packets_ptr_->packets.begin(), rdi_hrr_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); + return; + } + + rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (hrr_detection_list_callback_) { + hrr_detection_list_callback_(std::move(hrr_detection_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(rdi_hrr_packets_ptr_)); + } + + rdi_hrr_packets_ptr_ = std::make_unique(); + hrr_detection_list_ptr_ = + std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( + std::unique_ptr packet_msg) +{ + if (object_packets_ptr_->packets.size() != OBJECT_PACKET_NUM + 1) { + if (!first_object_packet_) { + PrintError("Incorrect number of object packages before CRC list"); + } + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); + return; + } + + uint16_t transmitted_crc = + (static_cast(packet_msg->data[5]) << 8) | packet_msg->data[6]; + uint16_t computed_crc = + crc16_packets(object_packets_ptr_->packets.begin(), object_packets_ptr_->packets.end(), 4); + + if (transmitted_crc != computed_crc) { + PrintError( + "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); + return; + } + + object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); + + if (object_list_callback_) { + object_list_callback_(std::move(object_list_ptr_)); + } + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(object_packets_ptr_)); + } + + object_packets_ptr_ = std::make_unique(); + object_list_ptr_ = std::make_unique(); +} + +void ContinentalSRR520Decoder::ProcessSensorStatusPacket( + std::unique_ptr packet_msg) +{ + static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); + + constexpr float STATUS_DISTANCE_RESOLUTION = 1e-3f; + constexpr float STATUS_DISTANCE_MIN_VALUE = -32.767; + constexpr float STATUS_ANGLE_RESOLUTION = 9.58766f; + constexpr float STATUS_ANGLE_MIN_VALUE = -3.14159f; + constexpr auto STATUS_ANGLE_STD_RESOLUTION = 1.52593e-05; + + StatusPacket status_packet; + std::memcpy(&status_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(status_packet)); + + auto diagnostic_array_msg_ptr = std::make_unique(); + + diagnostic_array_msg_ptr->header.frame_id = sensor_configuration_->frame_id; + diagnostic_array_msg_ptr->header.stamp = packet_msg->stamp; + diagnostic_array_msg_ptr->status.resize(1); + + auto & diagnostic_status = diagnostic_array_msg_ptr->status.front(); + auto & diagnostic_values = diagnostic_status.values; + diagnostic_values.reserve(33); + diagnostic_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + + diagnostic_status.message = "Sensor diagnostics for the SRR520"; + diagnostic_status.hardware_id = "SRR"; + diagnostic_status.name = "SRR"; + diagnostic_msgs::msg::KeyValue key_value; + + key_value.key = "time_stamp"; + key_value.value = std::to_string(status_packet.u_time_stamp.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_nsec"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_nsec.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "global_time_stamp_sync_status"; + key_value.value = std::to_string(status_packet.u_global_time_stamp_sync_status); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_global_time_stamp_sync_status != 1 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::stringstream sw_version_ss; + sw_version_ss << static_cast(status_packet.u_sw_version_major) << "." + << static_cast(status_packet.u_sw_version_minor) << "." + << static_cast(status_packet.u_sw_version_patch); + key_value.key = "sw_version_patch"; + key_value.value = sw_version_ss.str(); + diagnostic_values.push_back(key_value); + + key_value.key = "sensor_id"; + key_value.value = std::to_string(status_packet.u_sensor_id); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "lat_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_lat_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "vert_pos"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_vert_pos.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "long_pos_cog"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos_cog.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "wheelbase"; + key_value.value = std::to_string(STATUS_DISTANCE_RESOLUTION * status_packet.u_wheelbase.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "yaw_angle"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_yaw_angle.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "cover_damping"; + key_value.value = std::to_string( + STATUS_DISTANCE_RESOLUTION * status_packet.u_cover_damping.value() + STATUS_DISTANCE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + uint8_t plug_orientation = status_packet.u_plug_orientation & 0b1; + key_value.key = "plug_orientation"; + key_value.value = plug_orientation == 0 ? "0:PLUG_BOTTOM" : "1:PLUG_TOP"; + diagnostic_values.push_back(key_value); + + std::vector defective_message_vector; + + if (status_packet.u_defective & 0x01) { + defective_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_defective & 0x02) { + defective_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_defective & 0x04) { + defective_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_defective & 0x08) { + defective_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_defective & 0x10) { + defective_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_defective & 0x20) { + defective_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_defective & 0x40) { + defective_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_defective & 0x80) { + defective_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "defective"; + key_value.value = defective_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(defective_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_defective != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector supply_voltage_message_vector; + + if (status_packet.u_supply_voltage_limit & 0x01) { + supply_voltage_message_vector.push_back("Current Overvoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x02) { + supply_voltage_message_vector.push_back("Overvoltage Error this OPC"); + } + if (status_packet.u_supply_voltage_limit & 0x04) { + supply_voltage_message_vector.push_back("Current Undervoltage Error"); + } + if (status_packet.u_supply_voltage_limit & 0x08) { + supply_voltage_message_vector.push_back("Undervoltage Error this OPC"); + } + + key_value.key = "supply_voltage_limit"; + key_value.value = supply_voltage_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(supply_voltage_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_supply_voltage_limit != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector temperature_message_vector; + + if (status_packet.u_sensor_off_temp & 0x01) { + temperature_message_vector.push_back("Current Overtemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x02) { + temperature_message_vector.push_back("Overtemperature Error this OPC"); + } + if (status_packet.u_sensor_off_temp & 0x04) { + temperature_message_vector.push_back("Current Undertemperature Error"); + } + if (status_packet.u_sensor_off_temp & 0x08) { + temperature_message_vector.push_back("Undertemperature Error this OPC"); + } + + key_value.key = "sensor_off_temp"; + key_value.value = temperature_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(temperature_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_sensor_off_temp != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + auto valid_flag_to_string = [](uint8_t status) -> std::string { + std::vector status_vector; + if (status == 0) { + return "Ok"; + } + if (status > 3) { + return "Invalid value"; + } + if (status & 0x01) { + status_vector.push_back("Current error"); + } + if (status & 0x02) { + status_vector.push_back("Error this OPC"); + } + + return boost::algorithm::join(status_vector, ", "); + }; + + uint8_t u_long_vel_invalid = status_packet.u_dynamics_invalid0 & 0b00000011; + key_value.key = "long_vel_invalid"; + key_value.value = valid_flag_to_string(u_long_vel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00001100) >> 2; + key_value.key = "long_accel_invalid"; + key_value.value = valid_flag_to_string(u_long_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_lat_accel_invalid = (status_packet.u_dynamics_invalid0 & 0b00110000) >> 4; + key_value.key = "lat_accel_invalid"; + key_value.value = valid_flag_to_string(u_lat_accel_invalid); + diagnostic_values.push_back(key_value); + + uint8_t u_long_dir_invalid = (status_packet.u_dynamics_invalid0 & 0b11000000) >> 6; + key_value.key = "long_dir_invalid"; + key_value.value = valid_flag_to_string(u_long_dir_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid0 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + uint8_t u_yaw_rate_invalid = status_packet.u_dynamics_invalid1 & 0b00000011; + key_value.key = "yaw_rate_invalid"; + key_value.value = valid_flag_to_string(u_yaw_rate_invalid); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_dynamics_invalid1 != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::WARN + : diagnostic_status.level; + + std::vector ext_disturbed_message_vector; + + if (status_packet.u_ext_disturbed & 0x01) { + ext_disturbed_message_vector.push_back("Current Near Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x02) { + ext_disturbed_message_vector.push_back("Near Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x04) { + ext_disturbed_message_vector.push_back("Current HRR Scan RF Error"); + } + if (status_packet.u_ext_disturbed & 0x08) { + ext_disturbed_message_vector.push_back("HRR Scan RF this OPC"); + } + if (status_packet.u_ext_disturbed & 0x10) { + ext_disturbed_message_vector.push_back("Current RF HW Error"); + } + if (status_packet.u_ext_disturbed & 0x20) { + ext_disturbed_message_vector.push_back("RF HW Error this OPC"); + } + if (status_packet.u_ext_disturbed & 0x40) { + ext_disturbed_message_vector.push_back("Current HW Error"); + } + if (status_packet.u_ext_disturbed & 0x80) { + ext_disturbed_message_vector.push_back("HW Error this OPC"); + } + + key_value.key = "ext_disturbed"; + key_value.value = ext_disturbed_message_vector.size() == 0 + ? "Ok" + : boost::algorithm::join(ext_disturbed_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_ext_disturbed != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + key_value.key = "com_error"; + key_value.value = valid_flag_to_string(status_packet.u_com_error); + diagnostic_values.push_back(key_value); + diagnostic_status.level = status_packet.u_com_error != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + std::vector sw_message_vector; + uint8_t u_sw_error = status_packet.u_sw_error.value() & 0xff; + + if (u_sw_error & 0x01) { + sw_message_vector.push_back("Current Internal SW Error"); + } + if (u_sw_error & 0x02) { + sw_message_vector.push_back("Internal SW Error this OPC"); + } + if (u_sw_error & 0x04) { + sw_message_vector.push_back("Reset Error"); + } + if (u_sw_error & 0x08) { + sw_message_vector.push_back("Current Nvm Integrity"); + } + if (u_sw_error & 0x10) { + sw_message_vector.push_back("Nvm Integrity Error this OPC"); + } + if (u_sw_error & 0x20) { + sw_message_vector.push_back("RF HW Error this OPC"); + } + if (u_sw_error & 0x40) { + sw_message_vector.push_back("Runtime Error"); + } + if (u_sw_error & 0x80) { + sw_message_vector.push_back("Last Sensor Config Message Rejected Upper"); + } + + key_value.key = "sw_error"; + key_value.value = + sw_message_vector.size() == 0 ? "Ok" : boost::algorithm::join(sw_message_vector, ", "); + diagnostic_values.push_back(key_value); + diagnostic_status.level = (status_packet.u_sw_error.value() & 0x0f) != 0 + ? diagnostic_msgs::msg::DiagnosticStatus::ERROR + : diagnostic_status.level; + + bool aln_driving = status_packet.u_aln_status_azimuth_available & 0b00000001; + bool aln_sensor = (status_packet.u_aln_status_azimuth_available & 0b00000010) >> 1; + + key_value.key = "aln_status_driving"; + key_value.value = aln_driving ? "Driving conditions met" : "Driving conditions NOT met"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_status_sensor"; + key_value.value = aln_sensor ? "Sensor is adjusted" : "Sensor is not adjusted"; + diagnostic_values.push_back(key_value); + + key_value.key = "aln_azimuth_available"; + key_value.value = + std::to_string((status_packet.u_aln_status_azimuth_available & 0b00000100) >> 2); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth_std"; + key_value.value = + std::to_string(STATUS_ANGLE_STD_RESOLUTION * status_packet.u_aln_current_azimuth_std.value()); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_azimuth"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_azimuth.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + key_value.key = "aln_current_delta"; + key_value.value = std::to_string( + STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_delta.value() + STATUS_ANGLE_MIN_VALUE); + diagnostic_values.push_back(key_value); + + uint16_t computed_crc = crc16_packet(packet_msg->data.begin() + 4, packet_msg->data.end() - 3); + key_value.key = "crc_check"; + key_value.value = + std::to_string(status_packet.u_crc.value()) + "|" + std::to_string(computed_crc); + diagnostic_values.push_back(key_value); + + key_value.key = "sequence_counter"; + key_value.value = std::to_string(status_packet.u_sequence_counter); + diagnostic_values.push_back(key_value); + + if (status_callback_) { + status_callback_(std::move(diagnostic_array_msg_ptr)); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +void ContinentalSRR520Decoder::ProcessSyncFollowUpPacket( + std::unique_ptr packet_msg) +{ + if (sync_follow_up_callback_) { + sync_follow_up_callback_(packet_msg->stamp); + } + + auto nebula_packets = std::make_unique(); + nebula_packets->header.stamp = packet_msg->stamp; + nebula_packets->header.frame_id = sensor_configuration_->frame_id; + nebula_packets->packets.emplace_back(std::move(*packet_msg)); + + if (nebula_packets_callback_) { + nebula_packets_callback_(std::move(nebula_packets)); + } +} + +void ContinentalSRR520Decoder::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ptr_ = logger; +} + +void ContinentalSRR520Decoder::PrintInfo(std::string info) +{ + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSRR520Decoder::PrintError(std::string error) +{ + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSRR520Decoder::PrintDebug(std::string debug) +{ + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); + } else { + std::cout << debug << std::endl; + } +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 631d345dd..2577ea9a5 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(nebula_common) find_package(nebula_msgs) find_package(pandar_msgs) find_package(robosense_msgs) +find_package(ros2_socketcan) find_package(velodyne_msgs) # Common includes for all targets @@ -77,15 +78,17 @@ target_include_directories(nebula_hw_interfaces_robosense PUBLIC add_library(nebula_hw_interfaces_continental SHARED src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp + src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp ) target_link_libraries(nebula_hw_interfaces_continental PUBLIC ${boost_udp_driver_LIBRARIES} ${nebula_msgs_TARGETS} - + ${ros2_socketcan_LIBRARIES} ) target_include_directories(nebula_hw_interfaces_continental PUBLIC ${boost_udp_driver_INCLUDE_DIRS} ${nebula_msgs_INCLUDE_DIRS} + ${ros2_socketcan_INCLUDE_DIRS} ) install(TARGETS nebula_hw_interfaces_hesai EXPORT export_nebula_hw_interfaces_hesai) @@ -112,6 +115,7 @@ ament_export_dependencies( nebula_msgs pandar_msgs robosense_msgs + ros2_socketcan velodyne_msgs ) 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 ed578b66c..0563dc605 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 @@ -15,9 +15,10 @@ #ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H #define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + #include #include -#include #include #include @@ -56,7 +57,7 @@ class ContinentalARS548HwInterface /// @brief Registering callback /// @param callback Callback function /// @return Resulting status - Status RegisterCallback( + Status RegisterPacketCallback( std::function)> packet_callback); /// @brief Set the sensor mounting parameters @@ -133,10 +134,6 @@ class ContinentalARS548HwInterface /// @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); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp new file mode 100644 index 000000000..ddbbe1619 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -0,0 +1,148 @@ +// 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_CONTINENTAL_SRR520_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H + +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +/// @brief Hardware interface of the Continental SRR520 radar +class ContinentalSRR520HwInterface +{ +public: + /// @brief Constructor + ContinentalSRR520HwInterface(); + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart(); + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStop(); + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> + new_config_ptr); + + /// @brief Registering callback + /// @param callback Callback function + /// @return Resulting status + Status RegisterPacketCallback( + std::function)> packet_callback); + + /// @brief Sensor synchronization routine + void SensorSync(); + + /// @brief Process a new Sync Follow-up request + void SensorSyncFollowUp(builtin_interfaces::msg::Time stamp); + + /// @brief Configure the sensor + /// @param sensor_id Desired sensor id + /// @param longitudinal_autosar Desired longitudinal value in autosar coordinates + /// @param lateral_autosar Desired lateral value in AUTOSAR coordinates + /// @param vertical_autosar Desired vertical value in autosar coordinates + /// @param yaw_autosar Desired yaw value in autosar coordinates + /// @param longitudinal_cog Desired longitudinal cog + /// @param wheelbase Desired wheelbase + /// @param cover_damping Desired cover damping + /// @param plug_bottom Desired plug bottom + /// @param reset Rest the sensor to its default values + /// @return Resulting status + Status ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, + bool plug_bottom, bool reset); + + /// @brief Set the current vehicle dynamics + /// @param longitudinal_acceleration Longitudinal acceleration + /// @param lateral_acceleration Lateral acceleration + /// @param yaw_rate Yaw rate + /// @param longitudinal_velocity Longitudinal velocity + /// @param driving_direction Driving direction + /// @return Resulting status + Status SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); + +private: + /// @brief Send a Fd frame + /// @param data a buffer containing the data to send + template + bool SendFrame(const std::array & data, int can_frame_id); + + /// @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 Main loop of the CAN receiver thread + void ReceiveLoop(); + + std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_ptr_; + std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_; + std::unique_ptr receiver_thread_ptr_; + + std::shared_ptr config_ptr_; + std::function buffer)> + nebula_packet_callback_; + + std::mutex receiver_mutex_; + bool sensor_interface_active_{}; + + uint8_t sync_counter_{0}; + bool sync_follow_up_sent_{true}; + builtin_interfaces::msg::Time last_sync_stamp_; + + std::shared_ptr parent_node_logger_ptr_; +}; +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONTINENTAL_SRR520_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index f0167f6dc..fed686b8a 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -17,6 +17,7 @@ nebula_msgs pandar_msgs robosense_msgs + ros2_socketcan velodyne_msgs ament_cmake_gtest 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 bf05631b5..cd1053731 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 @@ -13,8 +13,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" + #include -#include namespace nebula { @@ -66,7 +67,7 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() return Status::OK; } -Status ContinentalARS548HwInterface::RegisterCallback( +Status ContinentalARS548HwInterface::RegisterPacketCallback( std::function)> packet_callback) { packet_callback_ = std::move(packet_callback); diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp new file mode 100644 index 000000000..bf7985bf0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -0,0 +1,405 @@ +// 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 "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp" + +#include +#include + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_srr520 +{ +ContinentalSRR520HwInterface::ContinentalSRR520HwInterface() +{ +} + +Status ContinentalSRR520HwInterface::SetSensorConfiguration( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> + new_config_ptr) +{ + config_ptr_ = new_config_ptr; + + return Status::OK; +} + +Status ContinentalSRR520HwInterface::SensorInterfaceStart() +{ + std::lock_guard lock(receiver_mutex_); + + try { + can_sender_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanSender>(config_ptr_->interface, true); + can_receiver_ptr_ = + std::make_unique<::drivers::socketcan::SocketCanReceiver>(config_ptr_->interface, true); + + can_receiver_ptr_->SetCanFilters( + ::drivers::socketcan::SocketCanReceiver::CanFilterList(config_ptr_->filters)); + PrintInfo(std::string("applied filters: ") + config_ptr_->filters); + + sensor_interface_active_ = true; + receiver_thread_ptr_ = + std::make_unique(&ContinentalSRR520HwInterface::ReceiveLoop, this); + } catch (const std::exception & ex) { + Status status = Status::CAN_CONNECTION_ERROR; + std::cerr << status << config_ptr_->interface << std::endl; + return status; + } + return Status::OK; +} + +template +bool ContinentalSRR520HwInterface::SendFrame(const std::array & data, int can_frame_id) +{ + ::drivers::socketcan::CanId send_id( + can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); + + try { + can_sender_ptr_->send_fd( + data.data(), data.size(), send_id, + std::chrono::duration_cast( + std::chrono::duration(config_ptr_->sender_timeout_sec))); + return true; + } catch (const std::exception & ex) { + PrintError(std::string("Error sending CAN message: ") + ex.what()); + return false; + } +} + +void ContinentalSRR520HwInterface::ReceiveLoop() +{ + ::drivers::socketcan::CanId receive_id{}; + std::chrono::nanoseconds receiver_timeout_nsec; + bool use_bus_time; + + while (true) { + auto packet_msg_ptr = std::make_unique(); + + { + std::lock_guard lock(receiver_mutex_); + receiver_timeout_nsec = std::chrono::duration_cast( + std::chrono::duration(config_ptr_->receiver_timeout_sec)); + use_bus_time = config_ptr_->use_bus_time; + + if (!sensor_interface_active_) { + break; + } + } + + try { + packet_msg_ptr->data.resize(68); // 64 bytes of data + 4 bytes of ID + receive_id = can_receiver_ptr_->receive_fd( + packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); + } catch (const std::exception & ex) { + PrintError(std::string("Error receiving CAN FD message: ") + ex.what()); + continue; + } + + packet_msg_ptr->data.resize(receive_id.length() + 4); + + uint32_t id = receive_id.identifier(); + packet_msg_ptr->data[0] = (id & 0xFF000000) >> 24; + packet_msg_ptr->data[1] = (id & 0x00FF0000) >> 16; + packet_msg_ptr->data[2] = (id & 0x0000FF00) >> 8; + packet_msg_ptr->data[3] = (id & 0x000000FF) >> 0; + + int64_t stamp = use_bus_time + ? static_cast(receive_id.get_bus_time() * 1000U) + : static_cast(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); + + packet_msg_ptr->stamp.sec = stamp / 1'000'000'000; + packet_msg_ptr->stamp.nanosec = stamp % 1'000'000'000; + + if (receive_id.frame_type() == ::drivers::socketcan::FrameType::ERROR) { + PrintError("CAN FD message is an error frame"); + continue; + } + + nebula_packet_callback_(std::move(packet_msg_ptr)); + } +} + +Status ContinentalSRR520HwInterface::RegisterPacketCallback( + std::function)> callback) +{ + nebula_packet_callback_ = std::move(callback); + return Status::OK; +} + +void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::Time stamp) +{ + if (!can_sender_ptr_) { + PrintError("Can sender is invalid so can not do follow up"); + } + + if (!config_ptr_->sync_use_bus_time || sync_follow_up_sent_) { + return; + } + + auto t0s = last_sync_stamp_; + t0s.nanosec = 0; + const auto & t1r = stamp; + + builtin_interfaces::msg::Time t4r = + rclcpp::Time(rclcpp::Time() + (rclcpp::Time(t1r) - rclcpp::Time(t0s))); + uint8_t t4r_seconds = static_cast(t4r.sec); + uint32_t t4r_nanoseconds = t4r.nanosec; + std::array data; + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = t4r_seconds & 0x3; // SGW and OVS + data[4] = (t4r_nanoseconds & 0xFF000000) >> 24; + data[5] = (t4r_nanoseconds & 0x00FF0000) >> 16; + data[6] = (t4r_nanoseconds & 0x0000FF00) >> 8; + data[7] = (t4r_nanoseconds & 0x000000FF) >> 0; + + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + sync_follow_up_sent_ = true; + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; +} + +void ContinentalSRR520HwInterface::SensorSync() +{ + if (!can_sender_ptr_) { + PrintError("Can sender is invalid so can not do sync up"); + return; + } + + if (!sync_follow_up_sent_) { + PrintError("We will send a SYNC message without having sent a FollowUp message first!"); + } + + 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(); + + builtin_interfaces::msg::Time stamp; + stamp.sec = static_cast(now_secs); + stamp.nanosec = static_cast(now_nanosecs % 1'000'000'000); + last_sync_stamp_ = stamp; + + std::array data; + data[0] = 0x20; // mode 0x10 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // use data + data[4] = (stamp.sec & 0xFF000000) >> 24; + data[5] = (stamp.sec & 0x00FF0000) >> 16; + data[6] = (stamp.sec & 0x0000FF00) >> 8; + data[7] = (stamp.sec & 0x000000FF) >> 0; + + std::array sync_crc_array{data[2], data[3], data[4], data[5], data[6], data[7], 0x00}; + uint8_t sync_crc = crc8h2f(sync_crc_array.begin(), sync_crc_array.end()); + data[1] = sync_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + if (config_ptr_->sync_use_bus_time) { + sync_follow_up_sent_ = false; + return; + } + + data[0] = 0x28; // mode 0x18 is without CRC + data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + (sync_counter_ & 0x0F); // Domain and counter + data[3] = 0; // SGW and OVS + data[4] = (stamp.nanosec & 0xFF000000) >> 24; + data[5] = (stamp.nanosec & 0x00FF0000) >> 16; + data[6] = (stamp.nanosec & 0x0000FF00) >> 8; + data[7] = (stamp.nanosec & 0x000000FF) >> 0; + + std::array follow_up_crc_array{data[2], data[3], data[4], data[5], + data[6], data[7], 0x00}; + uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); + data[1] = follow_up_crc; + + SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + + sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; + sync_follow_up_sent_ = true; +} + +Status ContinentalSRR520HwInterface::SensorInterfaceStop() +{ + { + std::lock_guard l(receiver_mutex_); + sensor_interface_active_ = false; + } + + receiver_thread_ptr_->join(); + return Status::ERROR_1; +} + +Status ContinentalSRR520HwInterface::ConfigureSensor( + uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, + float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, + bool reset) +{ + PrintInfo("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); + PrintInfo("lateral_autosar=" + std::to_string(lateral_autosar)); + PrintInfo("vertical_autosar=" + std::to_string(vertical_autosar)); + PrintInfo("longitudinal_cog=" + std::to_string(longitudinal_cog)); + PrintInfo("wheelbase=" + std::to_string(wheelbase)); + PrintInfo("yaw_autosar=" + std::to_string(yaw_autosar)); + PrintInfo("sensor_id=" + std::to_string(static_cast(sensor_id))); + PrintInfo("plug_bottom=" + std::to_string(plug_bottom)); + + if ( + longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || + lateral_autosar < -32.767f || lateral_autosar > 32.767f || vertical_autosar < -32.767f || + vertical_autosar > 32.767f || longitudinal_cog < -32.767f || longitudinal_cog > 32.767f || + wheelbase < 0.f || wheelbase > 65.534f || yaw_autosar < -3.14159f || yaw_autosar > 3.14159f || + cover_damping < -32.767f || cover_damping > 32.767f) { + PrintError("Sensor configuration values out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint16_t u_long_pos = static_cast((longitudinal_autosar + 32.767f) / 0.001f); + const uint16_t u_lat_pos = static_cast((lateral_autosar + 32.767f) / 0.001f); + const uint16_t u_vert_pos = static_cast((vertical_autosar + 32.767f) / 0.001f); + const uint16_t u_long_pos_cog = static_cast((longitudinal_cog + 32.767f) / 0.001f); + const uint16_t u_wheelbase = static_cast(wheelbase / 0.001f); + const uint16_t u_yaw_angle = static_cast((yaw_autosar + 3.14159f) / 9.5877e-05); + const uint16_t u_cover_damping = static_cast((cover_damping + 32.767f) / 0.001f); + + std::array data; + data[0] = sensor_id; + data[1] = static_cast((u_long_pos & 0xff00) >> 8); + data[2] = static_cast((u_long_pos & 0x00ff)); + + data[3] = static_cast((u_lat_pos & 0xff00) >> 8); + data[4] = static_cast((u_lat_pos & 0x00ff)); + + data[5] = static_cast((u_vert_pos & 0xff00) >> 8); + data[6] = static_cast((u_vert_pos & 0x00ff)); + + data[7] = static_cast((u_long_pos_cog & 0xff00) >> 8); + data[8] = static_cast((u_long_pos_cog & 0x00ff)); + + data[9] = static_cast((u_wheelbase & 0xff00) >> 8); + data[10] = static_cast((u_wheelbase & 0x00ff)); + + data[11] = static_cast((u_yaw_angle & 0xff00) >> 8); + data[12] = static_cast((u_yaw_angle & 0x00ff)); + + data[13] = static_cast((u_cover_damping & 0xff00) >> 8); + data[14] = static_cast((u_cover_damping & 0x00ff)); + + uint8_t plug_value = plug_bottom ? 0x00 : 0x01; + uint8_t reset_value = reset ? 0x80 : 0x00; + data[15] = plug_value | reset_value; + + if (SendFrame(data, SENSOR_CONFIG_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +Status ContinentalSRR520HwInterface::SetVehicleDynamics( + float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, + float longitudinal_velocity, bool standstill) +{ + if ( + longitudinal_acceleration < -12.7 || longitudinal_acceleration > 12.7 || + lateral_acceleration < -12.7 || lateral_acceleration > 12.7 || yaw_rate < -3.14159 || + yaw_rate > 3.14159 || abs(longitudinal_velocity) > 100.0) { + PrintError("Vehicle dynamics out of range!"); + return Status::SENSOR_CONFIG_ERROR; + } + + const uint8_t u_long_accel = static_cast((longitudinal_acceleration + 12.7) / 0.1); + const uint8_t u_lat_accel = static_cast((lateral_acceleration + 12.7) / 0.1); + const uint16_t u_yaw_rate = static_cast((yaw_rate + 3.14159) / 0.001534729); + const uint16_t u_long_vel = static_cast(std::abs(longitudinal_velocity) / 0.024425989); + uint8_t u_long_dir; + + if (standstill) { + u_long_dir = 0; + } else if (longitudinal_velocity > 0) { + u_long_dir = 1; + } else { + u_long_dir = 2; + } + + std::array data; + data[0] = u_long_accel; + data[1] = u_lat_accel; + data[2] = static_cast((u_yaw_rate & 0xff0) >> 4); + data[3] = (static_cast(u_yaw_rate & 0x0f) << 4) | + static_cast((u_long_vel & 0xf00) >> 8); + data[4] = static_cast(u_long_vel & 0xff); + data[5] = u_long_dir; + data[6] = 0x00; + data[7] = 0x00; + + if (SendFrame(data, VEH_DYN_CAN_MESSAGE_ID)) { + return Status::OK; + } else { + return Status::CAN_CONNECTION_ERROR; + } +} + +void ContinentalSRR520HwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger_ptr_ = logger; +} + +void ContinentalSRR520HwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger_ptr_) { + RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalSRR520HwInterface::PrintError(std::string error) +{ + if (parent_node_logger_ptr_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalSRR520HwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger_ptr_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); + } else { + std::cout << debug << std::endl; + } +} + +} // namespace continental_srr520 +} // namespace drivers +} // namespace nebula diff --git a/nebula_messages/continental_msgs/CMakeLists.txt b/nebula_messages/continental_msgs/CMakeLists.txt index c7b7978f6..724189b70 100644 --- a/nebula_messages/continental_msgs/CMakeLists.txt +++ b/nebula_messages/continental_msgs/CMakeLists.txt @@ -19,6 +19,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ContinentalArs548DetectionList.msg" "msg/ContinentalArs548Object.msg" "msg/ContinentalArs548ObjectList.msg" + "msg/ContinentalSrr520Detection.msg" + "msg/ContinentalSrr520DetectionList.msg" + "msg/ContinentalSrr520Object.msg" + "msg/ContinentalSrr520ObjectList.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg new file mode 100644 index 000000000..9536ffa12 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Detection.msg @@ -0,0 +1,11 @@ +float32 range +float32 azimuth_angle +float32 range_rate +float32 rcs +uint8 pdh00 +uint8 pdh01 +uint8 pdh02 +uint8 pdh03 +uint8 pdh04 +uint8 pdh05 +float32 snr diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg new file mode 100644 index 000000000..2bd8f85be --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520DetectionList.msg @@ -0,0 +1,10 @@ +std_msgs/Header header +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 v_ambiguous +float32 max_range + +ContinentalSrr520Detection[] detections diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg new file mode 100644 index 000000000..a12787fa9 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520Object.msg @@ -0,0 +1,21 @@ +uint8 object_id +float32 dist_x +float32 dist_y +float32 v_abs_x +float32 v_abs_y +float32 a_abs_x +float32 a_abs_y +float32 dist_x_std +float32 dist_y_std +float32 v_abs_x_std +float32 v_abs_y_std +float32 a_abs_x_std +float32 a_abs_y_std +float32 box_length +float32 box_width +float32 orientation +float32 rcs +float32 score +uint16 life_cycles +uint8 box_valid +uint8 object_status diff --git a/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg new file mode 100644 index 000000000..f873eb05a --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalSrr520ObjectList.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +uint32 internal_time_stamp_usec +uint8 global_time_stamp_sync_status +uint8 signal_status +uint8 sequence_counter +uint32 cycle_counter +float32 ego_vx +float32 ego_yaw_rate +uint8 motion_type + +ContinentalSrr520Object[] objects diff --git a/nebula_messages/continental_srvs/CMakeLists.txt b/nebula_messages/continental_srvs/CMakeLists.txt index ffa839825..b45d309f2 100644 --- a/nebula_messages/continental_srvs/CMakeLists.txt +++ b/nebula_messages/continental_srvs/CMakeLists.txt @@ -19,6 +19,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/ContinentalArs548SetVehicleParameters.srv" "srv/ContinentalArs548SetRadarParameters.srv" "srv/ContinentalArs548SetNetworkConfiguration.srv" + "srv/ContinentalSrr520SetRadarParameters.srv" DEPENDENCIES std_msgs ) diff --git a/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv new file mode 100644 index 000000000..4d0d3b297 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalSrr520SetRadarParameters.srv @@ -0,0 +1,15 @@ +bool autoconfigure_extrinsics 1 +float32 longitudinal +float32 lateral +float32 vertical +float32 yaw +float32 pitch +float32 vehicle_wheelbase -1.0 + +uint8 sensor_id # The new sensor id +float32 cover_damping # Cover damping in dB +bool plug_bottom # Whether the plug is in the bottom +bool reset_sensor_configuration +--- +bool success +string message # status messages diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 45cd6262b..709de2025 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -134,6 +134,8 @@ rclcpp_components_register_node(robosense_ros_wrapper ) ## Continental + +# ARS548 add_library(continental_ars548_ros_wrapper SHARED src/continental/continental_ars548_ros_wrapper.cpp src/continental/continental_ars548_decoder_wrapper.cpp @@ -173,10 +175,51 @@ rclcpp_components_register_node(continental_ars548_ros_wrapper EXECUTABLE continental_ars548_ros_wrapper_node ) +# SRR520 +add_library(continental_srr520_ros_wrapper SHARED + src/continental/continental_srr520_ros_wrapper.cpp + src/continental/continental_srr520_decoder_wrapper.cpp + src/continental/continental_srr520_hw_interface_wrapper.cpp + src/common/parameter_descriptors.cpp +) + +target_include_directories(continental_srr520_ros_wrapper PUBLIC + ${continental_msgs_INCLUDE_DIRS} + ${continental_srvs_INCLUDE_DIRS} + ${diagnostic_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${nebula_msgs_INCLUDE_DIRS} + ${nebula_decoders_INCLUDE_DIRS} + ${nebula_hw_interfaces_INCLUDE_DIRS} + ${radar_msgs_INCLUDE_DIRS} + ${rclcpp_components_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} + ${visualization_msgs_INCLUDE_DIRS} +) + +target_link_libraries(continental_srr520_ros_wrapper PUBLIC + ${continental_msgs_TARGETS} + ${continental_srvs_TARGETS} + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${nebula_msgs_TARGETS} + ${radar_msgs_TARGETS} + ${tf2_ros_TARGETS} + ${visualization_msgs_TARGETS} + nebula_decoders::nebula_decoders_continental + nebula_hw_interfaces::nebula_hw_interfaces_continental +) + +rclcpp_components_register_node(continental_srr520_ros_wrapper + PLUGIN "ContinentalSRR520RosWrapper" + EXECUTABLE continental_srr520_ros_wrapper_node +) + install(TARGETS hesai_ros_wrapper EXPORT export_hesai_ros_wrapper) install(TARGETS velodyne_ros_wrapper EXPORT export_velodyne_ros_wrapper) install(TARGETS robosense_ros_wrapper EXPORT export_robosense_ros_wrapper) install(TARGETS continental_ars548_ros_wrapper EXPORT export_continental_ars548_ros_wrapper) +install(TARGETS continental_srr520_ros_wrapper EXPORT export_continental_srr520_ros_wrapper) install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) if(BUILD_TESTING) diff --git a/nebula_ros/config/radar/continental/SRR520.param.yaml b/nebula_ros/config/radar/continental/SRR520.param.yaml new file mode 100644 index 000000000..f3e50f934 --- /dev/null +++ b/nebula_ros/config/radar/continental/SRR520.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + sensor_model: SRR520 + frame_id: continental + base_frame: base_link + interface: can4 + launch_hw: true + receiver_timeout_sec: 0.03 + sender_timeout_sec: 0.01 + filters: 0:0 # candump-like filters + use_bus_time: false + configuration_vehicle_wheelbase: 2.79 diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index f975a8fec..3c1eaa905 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -14,12 +14,13 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + #include #include #include #include -#include -#include #include #include @@ -148,7 +149,7 @@ class ContinentalARS548DecoderWrapper rclcpp::Publisher::SharedPtr objects_markers_pub_{}; rclcpp::Publisher::SharedPtr diagnostics_pub_{}; - std::unordered_set previous_ids_; + std::unordered_set previous_ids_{}; constexpr static int REFERENCE_POINTS_NUM = 9; constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { @@ -162,7 +163,7 @@ class ContinentalARS548DecoderWrapper {{0.0, -1.0}}, {{0.0, 0.0}}}}; - std::shared_ptr cloud_watchdog_; + std::shared_ptr watchdog_; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index 071b67126..855a3b5ba 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -14,9 +14,10 @@ #pragma once +#include "nebula_ros/common/parameter_descriptors.hpp" + #include #include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 1bd88ec75..ea9b3a822 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -14,14 +14,15 @@ #pragma once +#include "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" + #include #include #include #include -#include -#include -#include -#include #include #include @@ -57,10 +58,10 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::unique_ptr msg_ptr); + void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void ReceivePacketsMessageCallback(std::unique_ptr packets_msg); + void ReceivePacketsCallback(std::unique_ptr packets_msg_ptr); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp new file mode 100644 index 000000000..2dbfa3447 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -0,0 +1,162 @@ +// 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 "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/common/watchdog_timer.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSRR520DecoderWrapper +{ +public: + ContinentalSRR520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr); + + void ProcessPacket(std::unique_ptr packet_msg); + + void OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & + new_config_ptr); + + rcl_interfaces::msg::SetParametersResult OnParameterChange( + const std::vector & p); + + nebula::Status Status(); + + /// @brief Callback to process a new Near ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void NearDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new HRR ContinentalSrr520DetectionList from the driver + /// @param msg The new ContinentalSrr520DetectionList from the driver + void HRRDetectionListCallback( + std::unique_ptr msg); + + /// @brief Callback to process a new ContinentalSrr520ObjectList from the driver + /// @param msg The new ContinentalSrr520ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); + + /// @brief Callback to process a new DiagnosticArray from the driver + /// @param msg The new DiagnosticArray from the driver + void StatusCallback(std::unique_ptr msg); + + /// @brief Callback to process a new SyncFollowUp message from the driver + void SyncFollowUpCallback(builtin_interfaces::msg::Time stamp); + + /// @brief Callback to process a new NebulaPackets message from the driver + /// @param msg The new NebulaPackets from the driver + void PacketsCallback(std::unique_ptr msg); + +private: + nebula::Status InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config); + + /// @brief Convert SRR520 detections to a pointcloud + /// @param msg The SRR520 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a pointcloud + /// @param msg The SRR520 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 detections to a standard RadarScan msg + /// @param msg The SRR520 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + + /// @brief Convert SRR520 objects to a standard RadarTracks msg + /// @param msg The SRR520 object list msg + /// @return Resulting RadarTracks msg + radar_msgs::msg::RadarTracks ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + /// @brief Convert SRR520 objects to a standard MarkerArray msg + /// @param msg The SRR520 object list msg + /// @return Resulting MarkerArray msg + visualization_msgs::msg::MarkerArray ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + + const rclcpp::Node * const parent_node_; + nebula::Status status_; + rclcpp::Logger logger_; + + std::shared_ptr + sensor_cfg_{}; + + std::shared_ptr driver_ptr_{}; + std::shared_ptr hw_interface_ptr_{}; + std::mutex mtx_driver_ptr_; + + rclcpp::Publisher::SharedPtr packets_pub_{}; + + rclcpp::Publisher::SharedPtr + near_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + hrr_detection_list_pub_{}; + rclcpp::Publisher::SharedPtr + object_list_pub_{}; + rclcpp::Publisher::SharedPtr status_pub_{}; + rclcpp::Publisher::SharedPtr near_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr hrr_detection_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_{}; + rclcpp::Publisher::SharedPtr near_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr hrr_scan_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_raw_pub_{}; + rclcpp::Publisher::SharedPtr objects_markers_pub_{}; + + std::unordered_set previous_ids_{}; + + std::shared_ptr watchdog_; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp new file mode 100644 index 000000000..3da76cab6 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -0,0 +1,100 @@ +// 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 "nebula_ros/common/parameter_descriptors.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalSRR520HwInterfaceWrapper +{ +public: + ContinentalSRR520HwInterfaceWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config); + + /// @brief Starts the hw interface and subscribes to the input topics + void SensorInterfaceStart(); + + void OnConfigChange( + const std::shared_ptr & + new_config); + + /// @brief Get current status of the hw interface + /// @return Current status + nebula::Status Status(); + + std::shared_ptr HwInterface() const; + +private: + /// @brief Callback to send the odometry information to the radar device + /// @param odometry_msg The odometry message + /// @param acceleration_msg The acceleration message + void dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); + + /// @brief Service callback to set the new sensor mounting position + /// @param request Empty service request + /// @param response Empty service response + void ConfigureSensorRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); + + /// @brief Method periodically called to initiate the sensor synchronization mechanism + void syncTimerCallback(); + + rclcpp::Node * const parent_node_; + std::shared_ptr hw_interface_{}; + rclcpp::Logger logger_; + nebula::Status status_{}; + std::shared_ptr + config_ptr_{}; + + message_filters::Subscriber odometry_sub_; + message_filters::Subscriber acceleration_sub_; + + using ExactTimeSyncPolicy = message_filters::sync_policies::ExactTime< + geometry_msgs::msg::TwistWithCovarianceStamped, geometry_msgs::msg::AccelWithCovarianceStamped>; + using ExactTimeSync = message_filters::Synchronizer; + std::shared_ptr sync_ptr_{}; + rclcpp::TimerBase::SharedPtr sync_timer_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + configure_sensor_service_server_{}; +}; +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp new file mode 100644 index 000000000..6df11e9b6 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -0,0 +1,101 @@ +// 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 "nebula_ros/common/mt_queue.hpp" +#include "nebula_ros/common/parameter_descriptors.hpp" +#include "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" +#include "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ + +/// @brief Ros wrapper of continental srr520 driver +class ContinentalSRR520RosWrapper final : public rclcpp::Node +{ +public: + explicit ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalSRR520RosWrapper() 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 packet_msg_ptr); + + /// @brief Callback from replayed NebulaPackets + void ReceivePacketsCallback(std::unique_ptr packet_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/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index c5f7673b0..9e9ffe421 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -17,5 +17,16 @@ + + + + + + + + + + + diff --git a/nebula_ros/schema/SRR520.schema.json b/nebula_ros/schema/SRR520.schema.json new file mode 100644 index 000000000..19356adff --- /dev/null +++ b/nebula_ros/schema/SRR520.schema.json @@ -0,0 +1,69 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Radar Continental SRR520 parameters.", + "type": "object", + "definitions": { + "SRR520": { + "type": "object", + "properties": { + "interface": { + "$ref": "sub/communication.json#/definitions/interface" + }, + "receiver_timeout_sec": { + "$ref": "sub/communication.json#/definitions/receiver_timeout_sec" + }, + "sender_timeout_sec": { + "$ref": "sub/communication.json#/definitions/sender_timeout_sec" + }, + "filters": { + "$ref": "sub/communication.json#/definitions/filters" + }, + "launch_hw": { + "$ref": "sub/hardware.json#/definitions/launch_hw" + }, + "frame_id": { + "$ref": "sub/topic.json#/definitions/frame_id" + }, + "base_frame": { + "$ref": "sub/topic.json#/definitions/base_frame" + }, + "object_frame": { + "$ref": "sub/topic.json#/definitions/object_frame" + }, + "use_bus_time": { + "$ref": "sub/topic.json#/definitions/use_bus_time" + }, + "configuration_vehicle_wheelbase": { + "$ref": "sub/misc.json#/definitions/configuration_vehicle_wheelbase" + }, + "sensor_model": { + "$ref": "sub/radar_continental.json#/definitions/sensor_model" + } + }, + "required": [ + "interface", + "receiver_timeout_sec", + "sender_timeout_sec", + "filters", + "use_bus_time", + "launch_hw", + "configuration_vehicle_wheelbase", + "sensor_model" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/SRR520" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/nebula_ros/schema/sub/communication.json b/nebula_ros/schema/sub/communication.json index 315c6bfb6..b01c22e06 100644 --- a/nebula_ros/schema/sub/communication.json +++ b/nebula_ros/schema/sub/communication.json @@ -24,6 +24,11 @@ "readOnly": true, "description": "Sensor data port." }, + "filters": { + "type": "string", + "default": "0:0", + "description": "candump-style filters for CAN frames." + }, "gnss_port": { "type": "integer", "default": 2369, @@ -38,6 +43,11 @@ "readOnly": true, "description": "Host IPv4 address." }, + "interface": { + "type": "string", + "default": "can", + "description": "CAN interface name." + }, "multicast_ip": { "type": "string", "default": "224.0.0.2", @@ -62,11 +72,7 @@ "ptp_profile": { "type": "string", "default": "1588v2", - "enum": [ - "1588v2", - "802.1as", - "automotive" - ], + "enum": ["1588v2", "802.1as", "automotive"], "description": "PTP profile." }, "ptp_domain": { @@ -79,20 +85,26 @@ "ptp_transport_type": { "type": "string", "default": "UDP", - "enum": [ - "UDP", - "L2" - ], + "enum": ["UDP", "L2"], "description": "1588v2 supports 'UDP' or 'L2', other profiles only L2 (HW)." - }, + }, "ptp_switch_type": { "type": "string", "default": "TSN", - "enum": [ - "TSN", - "NON_TSN" - ], + "enum": ["TSN", "NON_TSN"], "description": "For automotive profile,'TSN' or 'NON_TSN'." + }, + "receiver_timeout_sec": { + "type": "number", + "default": 0.03, + "minimum": 0.0, + "description": "Timeout for reading data from the CAN bus." + }, + "sender_timeout_sec": { + "type": "number", + "default": 0.01, + "minimum": 0.0, + "description": "Timeout for sending data to the CAN bus." } } -} \ No newline at end of file +} diff --git a/nebula_ros/schema/sub/radar_continental.json b/nebula_ros/schema/sub/radar_continental.json index 2ccaff49e..cf499f2ba 100644 --- a/nebula_ros/schema/sub/radar_continental.json +++ b/nebula_ros/schema/sub/radar_continental.json @@ -7,7 +7,7 @@ "$ref": "hardware.json#/definitions/sensor_model", "enum": [ "ARS548", - "SSR520" + "SRR520" ] } } diff --git a/nebula_ros/schema/sub/topic.json b/nebula_ros/schema/sub/topic.json index b3aa90dd0..b20f859d3 100644 --- a/nebula_ros/schema/sub/topic.json +++ b/nebula_ros/schema/sub/topic.json @@ -34,6 +34,12 @@ "readOnly": true, "description": "Tracked objects frame." }, + "use_bus_time": { + "type": "boolean", + "default": false, + "readOnly": true, + "description": "Use CAN bus time for published sensor data." + }, "use_sensor_time": { "type": "boolean", "default": false, diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index 80f653d04..dede30210 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_decoder_wrapper.hpp" #include @@ -81,11 +81,10 @@ ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); - cloud_watchdog_ = + 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"); + RCLCPP_WARN_THROTTLE(logger_, *parent_node->get_clock(), 5000, "Missed output deadline"); }); } @@ -123,7 +122,7 @@ void ContinentalARS548DecoderWrapper::ProcessPacket( { driver_ptr_->ProcessPacket(std::move(packet_msg)); - cloud_watchdog_->update(); + watchdog_->update(); } void ContinentalARS548DecoderWrapper::DetectionListCallback( diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index 17e5b5ca8..9e6d3086f 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp" #include #include diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 6370c89c7..52f8c1df5 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "nebula_ros/continental/continental_ars548_ros_wrapper.hpp" namespace nebula { @@ -49,19 +49,18 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessPacket(std::move(packet_queue_.pop())); + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterCallback( + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( 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)); + std::bind(&ContinentalARS548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -77,146 +76,28 @@ 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()); - } + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.host_ip = declare_parameter("host_ip", param_read_only()); + config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); + config.multicast_ip = declare_parameter("multicast_ip", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.object_frame = declare_parameter("object_frame", param_read_write()); + config.data_port = static_cast(declare_parameter("data_port", param_read_only())); + config.configuration_host_port = + static_cast(declare_parameter("configuration_host_port", param_read_only())); + config.configuration_sensor_port = + static_cast(declare_parameter("configuration_sensor_port", param_read_only())); + config.use_sensor_time = declare_parameter("use_sensor_time", param_read_write()); + config.configuration_vehicle_length = static_cast( + declare_parameter("configuration_vehicle_length", param_read_write())); + config.configuration_vehicle_width = static_cast( + declare_parameter("configuration_vehicle_width", param_read_write())); + config.configuration_vehicle_height = static_cast( + declare_parameter("configuration_vehicle_height", param_read_write())); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_write())); if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -250,8 +131,8 @@ Status ContinentalARS548RosWrapper::ValidateAndSetConfig( return Status::OK; } -void ContinentalARS548RosWrapper::ReceivePacketsMessageCallback( - std::unique_ptr packets_msg) +void ContinentalARS548RosWrapper::ReceivePacketsCallback( + std::unique_ptr packets_msg_ptr) { if (hw_interface_wrapper_) { RCLCPP_ERROR_THROTTLE( @@ -261,15 +142,27 @@ void ContinentalARS548RosWrapper::ReceivePacketsMessageCallback( return; } - for (auto & packet : packets_msg->packets) { + for (auto & packet : packets_msg_ptr->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)); + nebula_packet_ptr->data = std::move(packet.data); packet_queue_.push(std::move(nebula_packet_ptr)); } } +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"); + } +} + Status ContinentalARS548RosWrapper::GetStatus() { return wrapper_status_; @@ -335,18 +228,6 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParamete 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/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp new file mode 100644 index 000000000..e85e6cdca --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -0,0 +1,603 @@ +// 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 "nebula_ros/continental/continental_srr520_decoder_wrapper.hpp" + +#include + +namespace nebula +{ +namespace ros +{ +ContinentalSRR520DecoderWrapper::ContinentalSRR520DecoderWrapper( + rclcpp::Node * const parent_node, + std::shared_ptr & + config, + std::shared_ptr hw_interface_ptr) +: parent_node_(parent_node), + status_(nebula::Status::NOT_INITIALIZED), + logger_(parent_node->get_logger().get_child("ContinentalSRR520Decoder")), + sensor_cfg_(config), + hw_interface_ptr_(hw_interface_ptr) +{ + using std::chrono_literals::operator""us; + if (!config) { + throw std::runtime_error( + "ContinentalSRR520DecoderWrapper cannot be instantiated without a valid config!"); + } + + RCLCPP_INFO(logger_, "Starting Decoder"); + + InitializeDriver(config); + status_ = driver_ptr_->GetStatus(); + + if (Status::OK != status_) { + throw std::runtime_error( + (std::stringstream() << "Error instantiating decoder: " << status_).str()); + } + + // Publish packets only if HW interface is connected + if (hw_interface_ptr_) { + packets_pub_ = parent_node->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + } + + auto qos_profile = rmw_qos_profile_sensor_data; + auto pointcloud_qos = + rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10), qos_profile); + + near_detection_list_pub_ = + parent_node->create_publisher( + "near_continental_detections", pointcloud_qos); + hrr_detection_list_pub_ = + parent_node->create_publisher( + "hrr_continental_detections", pointcloud_qos); + object_list_pub_ = + parent_node->create_publisher( + "continental_objects", pointcloud_qos); + status_pub_ = + parent_node->create_publisher("diagnostics", 10); + + near_detection_pointcloud_pub_ = parent_node->create_publisher( + "near_detection_points", pointcloud_qos); + hrr_detection_pointcloud_pub_ = parent_node->create_publisher( + "hrr_detection_points", pointcloud_qos); + object_pointcloud_pub_ = + parent_node->create_publisher("object_points", pointcloud_qos); + + near_scan_raw_pub_ = + parent_node->create_publisher("near_scan_raw", pointcloud_qos); + hrr_scan_raw_pub_ = + parent_node->create_publisher("hrr_scan_raw", pointcloud_qos); + + objects_raw_pub_ = + parent_node->create_publisher("objects_raw", pointcloud_qos); + + objects_markers_pub_ = + parent_node->create_publisher("marker_array", 10); + + RCLCPP_INFO_STREAM(logger_, ". Wrapper=" << status_); + + 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 sensor output deadline"); + }); +} + +Status ContinentalSRR520DecoderWrapper::InitializeDriver( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config) +{ + driver_ptr_.reset(); + driver_ptr_ = std::make_shared(config); + + driver_ptr_->RegisterNearDetectionListCallback(std::bind( + &ContinentalSRR520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterHRRDetectionListCallback(std::bind( + &ContinentalSRR520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalSRR520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalSRR520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); + + if (hw_interface_ptr_) { + driver_ptr_->RegisterSyncFollowUpCallback(std::bind( + &ContinentalSRR520DecoderWrapper::SyncFollowUpCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterPacketsCallback( + std::bind(&ContinentalSRR520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + } + + driver_ptr_->SetLogger( + std::make_shared(parent_node_->get_logger().get_child("Driver"))); + + return Status::OK; +} + +void ContinentalSRR520DecoderWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config) +{ + std::lock_guard lock(mtx_driver_ptr_); + InitializeDriver(new_config); + sensor_cfg_ = new_config; +} + +void ContinentalSRR520DecoderWrapper::ProcessPacket( + std::unique_ptr packet_msg) +{ + driver_ptr_->ProcessPacket(std::move(packet_msg)); + + watchdog_->update(); +} + +void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( + std::unique_ptr msg) +{ + if ( + near_detection_pointcloud_pub_->get_subscription_count() > 0 || + near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + near_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + near_scan_raw_pub_->get_subscription_count() > 0 || + near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + near_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + near_detection_list_pub_->get_subscription_count() > 0 || + near_detection_list_pub_->get_intra_process_subscription_count() > 0) { + near_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( + std::unique_ptr msg) +{ + if ( + hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || + hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + auto detection_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); + + detection_pointcloud_msg_ptr->header = msg->header; + hrr_detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + + if ( + hrr_scan_raw_pub_->get_subscription_count() > 0 || + hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); + } + + if ( + hrr_detection_list_pub_->get_subscription_count() > 0 || + hrr_detection_list_pub_->get_intra_process_subscription_count() > 0) { + hrr_detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::ObjectListCallback( + std::unique_ptr msg) +{ + if ( + object_pointcloud_pub_->get_subscription_count() > 0 || + object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + auto object_pointcloud_msg_ptr = std::make_unique(); + pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); + + object_pointcloud_msg_ptr->header = msg->header; + object_pointcloud_pub_->publish(std::move(object_pointcloud_msg_ptr)); + } + + if ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalSRR520DecoderWrapper::StatusCallback( + std::unique_ptr status_msg_ptr) +{ + status_pub_->publish(std::move(status_msg_ptr)); +} + +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + nebula::drivers::continental_srr520::PointSRR520Detection point{}; + for (const auto & detection : msg.detections) { + point.x = std::cos(detection.azimuth_angle) * detection.range; + point.y = std::sin(detection.azimuth_angle) * detection.range; + point.z = 0.f; + + point.azimuth = detection.azimuth_angle; + point.range = detection.range; + point.range_rate = detection.range_rate; + point.rcs = detection.rcs; + point.snr = detection.snr; + point.pdh00 = detection.pdh00; + point.pdh01 = detection.pdh01; + point.pdh02 = detection.pdh02; + point.pdh03 = detection.pdh03; + point.pdh04 = detection.pdh04; + point.pdh05 = detection.pdh05; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalSRR520DecoderWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_srr520::PointSRR520Object point{}; + for (const auto & object : msg.objects) { + point.x = object.dist_x; + point.y = object.dist_y; + point.z = 0.f; + + point.id = object.object_id; + point.age = object.life_cycles; + + point.box_length = object.box_length; + point.box_width = object.box_width; + + point.object_status = object.object_status; + point.orientation = object.orientation; + point.rcs = object.rcs; + point.score = object.score; + point.dynamics_abs_vel_x = object.v_abs_x; + point.dynamics_abs_vel_y = object.v_abs_y; + point.dynamics_abs_acc_x = object.a_abs_x; + point.dynamics_abs_acc_y = object.a_abs_y; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::ConvertToRadarScan( + const continental_msgs::msg::ContinentalSrr520DetectionList & msg) +{ + radar_msgs::msg::RadarScan output_msg; + output_msg.header = msg.header; + output_msg.returns.reserve(msg.detections.size()); + + radar_msgs::msg::RadarReturn return_msg; + for (const auto & detection : msg.detections) { + if (detection.pdh00 > 0 || detection.pdh00 > 1 || detection.pdh02 > 0) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = 0.f; + return_msg.doppler_velocity = detection.range_rate; + return_msg.amplitude = detection.rcs; + output_msg.returns.emplace_back(return_msg); + } + + return output_msg; +} + +radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTracks( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + radar_msgs::msg::RadarTracks output_msg; + output_msg.tracks.reserve(msg.objects.size()); + output_msg.header = msg.header; + + constexpr int16_t UNKNOWN_ID = 32000; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + track_msg.uuid.uuid[0] = static_cast(object.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((object.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((object.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((object.object_id >> 24) & 0xff); + + track_msg.position.x = static_cast(object.dist_x); + track_msg.position.y = static_cast(object.dist_y); + track_msg.position.z = 0.0; + + track_msg.velocity.x = static_cast(object.v_abs_x); + track_msg.velocity.y = static_cast(object.v_abs_y); + track_msg.velocity.z = 0.0; + track_msg.acceleration.x = static_cast(object.a_abs_x); + track_msg.acceleration.y = static_cast(object.a_abs_y); + track_msg.acceleration.z = 0.0; + track_msg.size.x = object.box_length; + track_msg.size.y = object.box_width; + track_msg.size.z = 1.f; + + track_msg.classification = UNKNOWN_ID; + + track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; + track_msg.position_covariance[1] = INVALID_COVARIANCE; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = INVALID_COVARIANCE; + + track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; + track_msg.velocity_covariance[1] = INVALID_COVARIANCE; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = INVALID_COVARIANCE; + + track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; + track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; + + track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[1] = 0.f; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalSrr520ObjectList & msg) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.reserve(4 * msg.objects.size()); + + constexpr int LINE_STRIP_CORNERS_NUM = 17; + constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + {{{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{1.0, 1.0, -1.0}}, + {{1.0, -1.0, -1.0}}, + {{-1.0, -1.0, -1.0}}, + {{-1.0, -1.0, 1.0}}, + {{1.0, -1.0, 1.0}}, + {{1.0, 1.0, 1.0}}, + {{-1.0, 1.0, 1.0}}, + {{-1.0, 1.0, -1.0}}, + {{1.0, 1.0, -1.0}}}}; + + constexpr int PALETTE_SIZE = 32; + constexpr std::array, PALETTE_SIZE> color_array = {{ + {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, + {{0.0, 0.0, 1.0}}, // Red, Green, Blue + {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, + {{1.0, 0.0, 1.0}}, // Yellow, Cyan, Magenta + {{1.0, 0.647, 0.0}}, {{0.749, 1.0, 0.0}}, + {{0.0, 0.502, 0.502}}, // Orange, Lime, Teal + {{0.502, 0.0, 0.502}}, {{1.0, 0.753, 0.796}}, + {{0.647, 0.165, 0.165}}, // Purple, Pink, Brown + {{0.502, 0.0, 0.0}}, {{0.502, 0.502, 0.0}}, + {{0.0, 0.0, 0.502}}, // Maroon, Olive, Navy + {{0.502, 0.502, 0.502}}, {{1.0, 0.4, 0.4}}, + {{0.4, 1.0, 0.4}}, // Grey, Light Red, Light Green + {{0.4, 0.4, 1.0}}, {{1.0, 1.0, 0.4}}, + {{0.4, 1.0, 1.0}}, // Light Blue, Light Yellow, Light Cyan + {{1.0, 0.4, 1.0}}, {{1.0, 0.698, 0.4}}, + {{0.698, 0.4, 1.0}}, // Light Magenta, Light Orange, Light Purple + {{1.0, 0.6, 0.8}}, {{0.71, 0.396, 0.114}}, + {{0.545, 0.0, 0.0}}, // Light Pink, Light Brown, Dark Red + {{0.0, 0.392, 0.0}}, {{0.0, 0.0, 0.545}}, + {{0.545, 0.545, 0.0}}, // Dark Green, Dark Blue, Dark Yellow + {{0.0, 0.545, 0.545}}, {{0.545, 0.0, 0.545}} // Dark Cyan, Dark Magenta + }}; + + std::unordered_set current_ids; + + for (const auto & object : msg.objects) { + if (!object.box_valid || object.object_status == 0) { + continue; + } + + const double half_length = 0.5 * object.box_length; + const double half_width = 0.5 * object.box_width; + constexpr double DEFAULT_HALF_SIZE = 1.0; + + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_->base_frame; + box_marker.header.stamp = msg.header.stamp; + box_marker.ns = "boxes"; + box_marker.id = object.object_id; + box_marker.action = visualization_msgs::msg::Marker::ADD; + box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + box_marker.lifetime = rclcpp::Duration::from_seconds(0); + box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; + box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; + box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.a = 1.0; + box_marker.scale.x = 0.1; + + box_marker.pose.position.x = object.dist_x; + box_marker.pose.position.y = object.dist_y; + box_marker.pose.position.z = DEFAULT_HALF_SIZE; + box_marker.pose.orientation.w = std::cos(0.5 * yaw); + box_marker.pose.orientation.z = std::sin(0.5 * yaw); + + for (const auto & corner : cube_corners) { + geometry_msgs::msg::Point p; + p.x = half_length * corner[0]; + p.y = half_width * corner[1]; + p.z = DEFAULT_HALF_SIZE * corner[2]; + box_marker.points.emplace_back(p); + } + + marker_array.markers.emplace_back(box_marker); + + visualization_msgs::msg::Marker text_marker = box_marker; + text_marker.ns = "object_age"; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.color.r = 1.0; + text_marker.color.g = 1.0; + text_marker.color.b = 1.0; + text_marker.color.a = 1.0; + text_marker.scale.x = 0.3; + text_marker.scale.y = 0.3; + text_marker.scale.z = 0.3; + text_marker.pose.position.z += 0.5; + text_marker.text = "ID=" + std::to_string(object.object_id) + + " Age=" + std::to_string(object.life_cycles) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) << "\n" + << static_cast(object.box_length) << "/" + << static_cast(object.object_status); + + text_marker.ns = "object_status"; + text_marker.text = object_status_ss.str(); + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_dynamics_ss; + object_dynamics_ss << std::fixed << std::setprecision(3) + << "ID=" << static_cast(object.object_id) + << "\nyaw=" << object.orientation << "\nvx=" << object.v_abs_x + << "\nvy=" << object.v_abs_y << "\nax=" << object.a_abs_x + << "\nay=" << object.a_abs_y; + + text_marker.ns = "object_dynamics"; + text_marker.text = object_dynamics_ss.str(); + + marker_array.markers.emplace_back(text_marker); + } + + for (const auto & previous_id : previous_ids_) { + if (current_ids.find(previous_id) != current_ids.end()) { + continue; + } + + visualization_msgs::msg::Marker delete_marker; + delete_marker.header.frame_id = sensor_cfg_->base_frame; + delete_marker.header.stamp = msg.header.stamp; + delete_marker.ns = "boxes"; + delete_marker.id = previous_id; + delete_marker.action = visualization_msgs::msg::Marker::DELETE; + + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_age"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_status"; + marker_array.markers.push_back(delete_marker); + + delete_marker.ns = "object_dynamics"; + marker_array.markers.push_back(delete_marker); + } + + previous_ids_.clear(); + previous_ids_ = current_ids; + + return marker_array; +} + +void ContinentalSRR520DecoderWrapper::SyncFollowUpCallback(builtin_interfaces::msg::Time stamp) +{ + hw_interface_ptr_->SensorSyncFollowUp(stamp); +} + +void ContinentalSRR520DecoderWrapper::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)); + } +} + +nebula::Status ContinentalSRR520DecoderWrapper::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_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp new file mode 100644 index 000000000..42745fa58 --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -0,0 +1,183 @@ +// 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 "nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ + +ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( + 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 ContinentalSRR520HwInterfaceWrapper::SensorInterfaceStart() +{ + using std::chrono_literals::operator""ms; + + if (Status::OK == status_) { + hw_interface_->SensorInterfaceStart(); + } + + if (Status::OK == status_) { + odometry_sub_.subscribe(parent_node_, "odometry_input"); + acceleration_sub_.subscribe(parent_node_, "acceleration_input"); + + sync_ptr_ = + std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); + sync_ptr_->registerCallback(&ContinentalSRR520HwInterfaceWrapper::dynamicsCallback, this); + + configure_sensor_service_server_ = + parent_node_->create_service( + "configure_sensor", std::bind( + &ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); + + sync_timer_ = rclcpp::create_timer( + parent_node_, parent_node_->get_clock(), 100ms, + std::bind(&ContinentalSRR520HwInterfaceWrapper::syncTimerCallback, this)); + } +} + +void ContinentalSRR520HwInterfaceWrapper::OnConfigChange( + const std::shared_ptr< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & + new_config_ptr) +{ + hw_interface_->SetSensorConfiguration(new_config_ptr); + config_ptr_ = new_config_ptr; +} + +Status ContinentalSRR520HwInterfaceWrapper::Status() +{ + return status_; +} + +std::shared_ptr +ContinentalSRR520HwInterfaceWrapper::HwInterface() const +{ + return hw_interface_; +} + +void ContinentalSRR520HwInterfaceWrapper::dynamicsCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) +{ + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(odometry_msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(odometry_msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + hw_interface_->SetVehicleDynamics( + acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, + odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); +} + +void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( + 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 vehicle_wheelbase = request->vehicle_wheelbase; + + 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; + } + + 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 - vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + } + + yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); + + auto result = hw_interface_->ConfigureSensor( + request->sensor_id, longitudinal, lateral, vertical, yaw, + longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, + request->plug_bottom, request->reset_sensor_configuration); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalSRR520HwInterfaceWrapper::syncTimerCallback() +{ + hw_interface_->SensorSync(); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp new file mode 100644 index 000000000..8fff84139 --- /dev/null +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -0,0 +1,218 @@ +// 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 "nebula_ros/continental/continental_srr520_ros_wrapper.hpp" + +namespace nebula +{ +namespace ros +{ +ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptions & options) +: rclcpp::Node( + "continental_srr520_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_, hw_interface_wrapper_->HwInterface()); + } else { + decoder_wrapper_.emplace(this, config_ptr_, nullptr); + } + + RCLCPP_DEBUG(get_logger(), "Starting stream"); + + decoder_thread_ = std::thread([this]() { + while (true) { + decoder_wrapper_->ProcessPacket(packet_queue_.pop()); + } + }); + + if (launch_hw_) { + hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); + StreamStart(); + } else { + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind(&ContinentalSRR520RosWrapper::ReceivePacketsCallback, 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(&ContinentalSRR520RosWrapper::OnParameterChange, this, std::placeholders::_1)); +} + +nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() +{ + nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration config; + + config.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", param_read_only())); + config.interface = declare_parameter("interface", param_read_only()); + config.receiver_timeout_sec = + static_cast(declare_parameter("receiver_timeout_sec", param_read_only())); + config.sender_timeout_sec = + static_cast(declare_parameter("sender_timeout_sec", param_read_only())); + config.filters = declare_parameter("filters", param_read_only()); + config.frame_id = declare_parameter("frame_id", param_read_write()); + config.base_frame = declare_parameter("base_frame", param_read_write()); + config.use_bus_time = declare_parameter("use_bus_time", param_read_only()); + config.configuration_vehicle_wheelbase = static_cast( + declare_parameter("configuration_vehicle_wheelbase", param_read_only())); + + if (config.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(config); + return ValidateAndSetConfig(new_config_ptr); +} + +Status ContinentalSRR520RosWrapper::ValidateAndSetConfig( + std::shared_ptr & + new_config) +{ + if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (new_config->frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + if (hw_interface_wrapper_) { + hw_interface_wrapper_->OnConfigChange(new_config); + } + if (decoder_wrapper_) { + decoder_wrapper_->OnConfigChange(new_config); + } + + config_ptr_ = new_config; + return Status::OK; +} + +void ContinentalSRR520RosWrapper::ReceivePacketsCallback( + 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; + nebula_packet_ptr->data = std::move(packet.data); + + packet_queue_.push(std::move(nebula_packet_ptr)); + } +} + +void ContinentalSRR520RosWrapper::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"); + } +} + +Status ContinentalSRR520RosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalSRR520RosWrapper::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 ContinentalSRR520RosWrapper::OnParameterChange( + const std::vector & p) +{ + using rcl_interfaces::msg::SetParametersResult; + + if (p.empty()) { + return rcl_interfaces::build().successful(true).reason(""); + } + + RCLCPP_INFO(get_logger(), "OnParameterChange"); + + drivers::continental_srr520::ContinentalSRR520SensorConfiguration 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, "use_bus_time", new_config.use_bus_time) | + get_param(p, "configuration_vehicle_wheelbase", new_config.configuration_vehicle_wheelbase); + + if (!got_any) { + return rcl_interfaces::build().successful(true).reason(""); + } + + auto new_config_ptr = std::make_shared< + const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(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(""); +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalSRR520RosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/CMakeLists.txt b/nebula_tests/continental/CMakeLists.txt index fa28a594d..10e01ca1f 100644 --- a/nebula_tests/continental/CMakeLists.txt +++ b/nebula_tests/continental/CMakeLists.txt @@ -1,6 +1,7 @@ # Continental ARS548 add_library(continental_ros_decoder_test_ars548 SHARED continental_ros_decoder_test_ars548.cpp + parameter_descriptors.cpp ) target_include_directories(continental_ros_decoder_test_ars548 PUBLIC @@ -23,3 +24,29 @@ target_include_directories(continental_ros_decoder_test_main_ars548 PUBLIC target_link_libraries(continental_ros_decoder_test_main_ars548 continental_ros_decoder_test_ars548 ) +# Continental SRR520 +add_library(continental_ros_decoder_test_srr520 SHARED + continental_ros_decoder_test_srr520.cpp + parameter_descriptors.cpp +) + +target_include_directories(continental_ros_decoder_test_srr520 PUBLIC + ${NEBULA_TEST_INCLUDE_DIRS} +) + +target_link_libraries(continental_ros_decoder_test_srr520 + ${CONTINENTAL_TEST_LIBRARIES} +) + +ament_add_gtest(continental_ros_decoder_test_main_srr520 + continental_ros_decoder_test_main_srr520.cpp +) + +target_include_directories(continental_ros_decoder_test_main_srr520 PUBLIC + ${PROJECT_SOURCE_DIR}/src/continental + include + ${NEBULA_TEST_INCLUDE_DIRS} +) +target_link_libraries(continental_ros_decoder_test_main_srr520 + continental_ros_decoder_test_srr520 +) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 1bd913f62..727b9db62 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -14,17 +14,19 @@ #include "continental_ros_decoder_test_ars548.hpp" -#include "rclcpp/serialization.hpp" -#include "rclcpp/serialized_message.hpp" -#include "rcpputils/filesystem_helper.hpp" -#include "rcutils/time.h" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/storage_options.hpp" +#include "parameter_descriptors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include #include +#include #include #include @@ -88,82 +90,22 @@ Status ContinentalRosDecoderTest::GetParameters( std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", "ARS548"); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("base_frame", "some_base_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("object_frame", "some_object_frame", descriptor); - sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "some_sensor_frame", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = 4; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter( - "target_topic", "/sensing/radar/front_center/nebula_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); - } + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "ARS548", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + sensor_configuration.object_frame = + declare_parameter("object_frame", "some_object_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_center/nebula_packets", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -217,7 +159,7 @@ void ContinentalRosDecoderTest::DetectionListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_detection.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -232,7 +174,7 @@ void ContinentalRosDecoderTest::ObjectListCallback( std::stringstream detection_path; detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; - auto gt_path = rcpputils::fs::path(bag_path).parent_path() / detection_path.str(); + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); CheckResult(msg_as_string, gt_path.string()); @@ -243,22 +185,22 @@ void ContinentalRosDecoderTest::ReadBag() rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; - auto target_topic_name = target_topic; + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); - rcpputils::fs::path bag_dir(bag_path); + rcpputils::fs::path bag_dir(bag_path_); - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; rclcpp::Serialization serialization; { @@ -269,7 +211,7 @@ void ContinentalRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { nebula_msgs::msg::NebulaPackets extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index bf79912b7..64e53c04b 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -15,12 +15,11 @@ #ifndef NEBULA_ContinentalRosDecoderTestArs548_H #define NEBULA_ContinentalRosDecoderTestArs548_H -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" - #include +#include +#include +#include +#include #include #include @@ -75,10 +74,10 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test void ReadBag(); private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; }; } // namespace ros diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp new file mode 100644 index 000000000..56b072750 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -0,0 +1,60 @@ +// 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 "continental_ros_decoder_test_srr520.hpp" + +#include + +#include + +#include + +std::shared_ptr continental_driver; + +TEST(TestDecoder, TestBag) +{ + std::cout << "TEST(TestDecoder, TestBag)" << std::endl; + continental_driver->ReadBag(); +} + +int main(int argc, char * argv[]) +{ + std::cout << "continental_ros_decoder_test_main_srr520.cpp" << std::endl; + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + std::string node_name = "nebula_continental_decoder_test"; + + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + + continental_driver = std::make_shared(options, node_name); + exec.add_node(continental_driver->get_node_base_interface()); + + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); + nebula::Status driver_status = continental_driver->GetStatus(); + int result = 0; + if (driver_status == nebula::Status::OK) { + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); + result = RUN_ALL_TESTS(); + } else { + RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); + } + RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Ending"); + continental_driver.reset(); + rclcpp::shutdown(); + + return result; +} diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp new file mode 100644 index 000000000..7991fada1 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -0,0 +1,259 @@ +// 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 "continental_ros_decoder_test_srr520.hpp" + +#include "parameter_descriptors.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalRosDecoderTest::ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name) +: rclcpp::Node(node_name, options) +{ + drivers::continental_srr520::ContinentalSRR520SensorConfiguration sensor_configuration; + + wrapper_status_ = GetParameters(sensor_configuration); + if (Status::OK != wrapper_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << wrapper_status_); + return; + } + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); + + sensor_cfg_ptr_ = + std::make_shared( + sensor_configuration); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast( + sensor_cfg_ptr_)); + + driver_ptr_->RegisterHRRDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::HRRDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterNearDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::NearDetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalRosDecoderTest::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterStatusCallback( + std::bind(&ContinentalRosDecoderTest::StatusCallback, this, std::placeholders::_1)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); +} + +Status ContinentalRosDecoderTest::InitializeDriver( + std::shared_ptr + sensor_configuration) +{ + // driver should be initialized here with proper decoder + driver_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + return Status::OK; +} + +Status ContinentalRosDecoderTest::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalRosDecoderTest::GetParameters( + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration) +{ + std::filesystem::path bag_root_dir = + _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; + bag_root_dir /= "continental"; + + sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + declare_parameter("sensor_model", "SRR520", param_read_only())); + sensor_configuration.frame_id = + declare_parameter("frame_id", "some_sensor_frame", param_read_only()); + sensor_configuration.base_frame = + declare_parameter("base_frame", "some_base_frame", param_read_only()); + + bag_path_ = declare_parameter( + "bag_path", (bag_root_dir / "srr520" / "1708578209").string(), param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter( + "target_topic", "/sensing/radar/front_left/nebula_packets", param_read_only()); + + 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 ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAML::Node & node2) +{ + ASSERT_EQ(node1.IsDefined(), node2.IsDefined()); + ASSERT_EQ(node1.IsMap(), node2.IsMap()); + ASSERT_EQ(node1.IsNull(), node2.IsNull()); + ASSERT_EQ(node1.IsScalar(), node2.IsScalar()); + ASSERT_EQ(node1.IsSequence(), node2.IsSequence()); + + if (node1.IsMap()) { + for (YAML::const_iterator it = node1.begin(); it != node1.end(); ++it) { + CompareNodes(it->second, node2[it->first.as()]); + } + } else if (node1.IsScalar()) { + ASSERT_EQ(node1.as(), node2.as()); + } else if (node1.IsSequence()) { + ASSERT_EQ(node1.size(), node2.size()); + for (std::size_t i = 0; i < node1.size(); i++) { + CompareNodes(node1[i], node2[i]); + } + } +} + +void ContinentalRosDecoderTest::CheckResult( + const std::string msg_as_string, const std::string & gt_path) +{ + YAML::Node current_node = YAML::Load(msg_as_string); + YAML::Node gt_node = YAML::LoadFile(gt_path); + CompareNodes(gt_node, current_node); + + // To generate the gt + // std::cout << gt_path << std::endl; + // std::ofstream ostream(gt_path); + // ostream << msg_as_string; + // ostream.close(); +} + +void ContinentalRosDecoderTest::HRRDetectionListCallback( + std::unique_ptr msg) +{ + hrr_detection_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_hrr_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::NearDetectionListCallback( + std::unique_ptr msg) +{ + near_detection_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec + << "_near_detection.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ObjectListCallback( + std::unique_ptr msg) +{ + object_list_count_++; + EXPECT_EQ(sensor_cfg_ptr_->base_frame, msg->header.frame_id); + std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); + + std::stringstream detection_path; + detection_path << msg->header.stamp.sec << "_" << msg->header.stamp.nanosec << "_object.yaml"; + + auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); + ASSERT_TRUE(gt_path.exists()); + + CheckResult(msg_as_string, gt_path.string()); +} + +void ContinentalRosDecoderTest::ReadBag() +{ + rosbag2_storage::StorageOptions storage_options; + rosbag2_cpp::ConverterOptions converter_options; + + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; + + auto target_topic_name = target_topic_; + if (target_topic_name.substr(0, 1) == "/") { + target_topic_name = target_topic_name.substr(1); + } + target_topic_name = std::regex_replace(target_topic_name, std::regex("/"), "_"); + + rcpputils::fs::path bag_dir(bag_path_); + + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; + rclcpp::Serialization serialization; + + { + rosbag2_cpp::Reader bag_reader(std::make_unique()); + bag_reader.open(storage_options, converter_options); + while (bag_reader.has_next()) { + auto bag_message = bag_reader.read_next(); + + std::cout << "Found topic name " << bag_message->topic_name << std::endl; + + if (bag_message->topic_name == target_topic_) { + nebula_msgs::msg::NebulaPackets extracted_msg; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); + serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); + + std::cout << "Found data in topic " << bag_message->topic_name << ": " + << bag_message->time_stamp << std::endl; + + for (auto & packet_msg : extracted_msg.packets) { + auto extracted_msg_ptr = std::make_unique(packet_msg); + driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); + } + } + } + } + + EXPECT_EQ(1, near_detection_list_count_); + EXPECT_EQ(1, hrr_detection_list_count_); + EXPECT_EQ(1, object_list_count_); +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp new file mode 100644 index 000000000..525360e03 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -0,0 +1,96 @@ +// 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_ContinentalRosDecoderTestsrr520_H +#define NEBULA_ContinentalRosDecoderTestsrr520_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test +{ + std::shared_ptr driver_ptr_; + Status wrapper_status_; + + std::shared_ptr + sensor_cfg_ptr_; + + Status InitializeDriver( + std::shared_ptr + sensor_configuration); + + Status GetParameters( + drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration); + + void CheckResult(const std::string msg_as_string, const std::string & gt_path); + + void HRRDetectionListCallback( + std::unique_ptr msg); + + void NearDetectionListCallback( + std::unique_ptr msg); + + void StatusCallback([[maybe_unused]] std::unique_ptr msg) + { + } + + void ObjectListCallback(std::unique_ptr msg); + + void CompareNodes(const YAML::Node & node1, const YAML::Node & node2); + + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + +public: + explicit ContinentalRosDecoderTest( + const rclcpp::NodeOptions & options, const std::string & node_name); + + void ReceiveScanMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + Status GetStatus(); + void ReadBag(); + +private: + std::string bag_path_{}; + std::string storage_id_{}; + std::string format_{}; + std::string target_topic_{}; + std::size_t near_detection_list_count_{}; + std::size_t hrr_detection_list_count_{}; + std::size_t object_list_count_{}; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRosDecoderTestsrr520_H diff --git a/nebula_tests/continental/parameter_descriptors.cpp b/nebula_tests/continental/parameter_descriptors.cpp new file mode 100644 index 000000000..d2d265780 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.cpp @@ -0,0 +1,34 @@ +#include "parameter_descriptors.hpp" + +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write() +{ + return rcl_interfaces::msg::ParameterDescriptor{}; +}; + +rcl_interfaces::msg::ParameterDescriptor param_read_only() +{ + return rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); +} + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step) +{ + return { + rcl_interfaces::msg::FloatingPointRange().set__from_value(start).set__to_value(stop).set__step( + step)}; +} + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step) +{ + return { + rcl_interfaces::msg::IntegerRange().set__from_value(start).set__to_value(stop).set__step(step)}; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/parameter_descriptors.hpp b/nebula_tests/continental/parameter_descriptors.hpp new file mode 100644 index 000000000..6d4e38504 --- /dev/null +++ b/nebula_tests/continental/parameter_descriptors.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include + +#include +#include +namespace nebula +{ +namespace ros +{ + +rcl_interfaces::msg::ParameterDescriptor param_read_write(); + +rcl_interfaces::msg::ParameterDescriptor param_read_only(); + +rcl_interfaces::msg::ParameterDescriptor::_floating_point_range_type float_range( + double start, double stop, double step); + +rcl_interfaces::msg::ParameterDescriptor::_integer_range_type int_range( + int start, int stop, int step); + +/// @brief Get a parameter's value from a list of parameters, if that parameter is in the list. +/// @tparam T The parameter's expected value type +/// @param p A vector of parameters +/// @param name Target parameter name +/// @param value (out) Parameter value. Set if parameter is found, left untouched otherwise. +/// @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; +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml new file mode 100644 index 000000000..f9d7cfa56 --- /dev/null +++ b/nebula_tests/data/continental/srr520/1708578208_992410284_object.yaml @@ -0,0 +1,854 @@ +header: + stamp: + sec: 1708578208 + nanosec: 992410284 + frame_id: some_base_frame +internal_time_stamp_usec: 468502487 +global_time_stamp_sync_status: 1 +signal_status: 1 +sequence_counter: 198 +cycle_counter: 451 +ego_vx: 6.78121 +ego_yaw_rate: -0.00632930 +motion_type: 1 +objects: + - object_id: 8 + dist_x: 26.9082 + dist_y: 1.72125 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.12653 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 0 + dist_x: 9.18302 + dist_y: 4.48623 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0714155 + dist_y_std: 0.104376 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.417582 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: 0.439667 + score: 100.000 + life_cycles: 78 + box_valid: 1 + object_status: 2 + - object_id: 4 + dist_x: 0.503561 + dist_y: 5.72223 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 2.02736 + score: 100.000 + life_cycles: 9 + box_valid: 0 + object_status: 2 + - object_id: 11 + dist_x: 43.1043 + dist_y: -0.119017 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.203259 + dist_y_std: 1.38986 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00732601 + box_width: 0.146520 + orientation: 2.63000e-07 + rcs: 21.8857 + score: 86.6667 + life_cycles: 3 + box_valid: 1 + object_status: 2 + - object_id: 5 + dist_x: 5.29191 + dist_y: 15.8757 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.159311 + dist_y_std: 0.0878960 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.981685 + orientation: 2.63000e-07 + rcs: 8.74450 + score: 100.000 + life_cycles: 38 + box_valid: 1 + object_status: 3 + - object_id: 9 + dist_x: 27.4392 + dist_y: -5.39262 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 15.9257 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 10 + dist_x: 4.79751 + dist_y: 16.0405 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -3.59062 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 7 + dist_x: -2.04168 + dist_y: 6.11591 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.102545 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.388278 + box_width: 0.446886 + orientation: 2.63000e-07 + rcs: -3.37079 + score: 100.000 + life_cycles: 25 + box_valid: 1 + object_status: 2 + - object_id: 2 + dist_x: 4.43129 + dist_y: 16.3427 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.64387 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 6 + dist_x: 13.5502 + dist_y: 10.6754 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.355246 + dist_y_std: 0.379051 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.161172 + box_width: 0.205128 + orientation: 2.63000e-07 + rcs: 1.58769 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 14 + dist_x: 3.85449 + dist_y: 21.4057 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -13.2145 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 3 + dist_x: 12.7354 + dist_y: 4.01929 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.4543 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 23 + dist_x: 7.93787 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.278388 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -2.12506 + score: 100.000 + life_cycles: 46 + box_valid: 1 + object_status: 2 + - object_id: 12 + dist_x: -0.292973 + dist_y: 6.02436 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.106208 + dist_y_std: 0.0805713 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.234432 + box_width: 0.344322 + orientation: 2.63000e-07 + rcs: -4.29897 + score: 100.000 + life_cycles: 82 + box_valid: 1 + object_status: 2 + - object_id: 17 + dist_x: 29.3527 + dist_y: -5.52995 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.0293040 + box_width: 0.0586081 + orientation: 2.63000e-07 + rcs: 24.4504 + score: 73.3333 + life_cycles: 2 + box_valid: 1 + object_status: 2 + - object_id: 15 + dist_x: 3.66223 + dist_y: 19.8401 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.59209 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 16 + dist_x: 44.7249 + dist_y: 1.68463 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 13.5076 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 32 + dist_x: 9.12809 + dist_y: 10.6388 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.108039 + dist_y_std: 0.0842336 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.996337 + orientation: 2.63000e-07 + rcs: 3.59062 + score: 100.000 + life_cycles: 39 + box_valid: 1 + object_status: 2 + - object_id: 20 + dist_x: 3.69885 + dist_y: 15.4180 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.172130 + dist_y_std: 0.113532 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.732601 + box_width: 0.783883 + orientation: 2.63000e-07 + rcs: -0.952614 + score: 86.6667 + life_cycles: 70 + box_valid: 1 + object_status: 3 + - object_id: 35 + dist_x: 11.0874 + dist_y: 4.29396 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0677531 + dist_y_std: 0.119026 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 1.00366 + orientation: 2.63000e-07 + rcs: -2.54030 + score: 100.000 + life_cycles: 56 + box_valid: 1 + object_status: 2 + - object_id: 18 + dist_x: 4.02845 + dist_y: 17.7709 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -15.1685 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 19 + dist_x: 20.6641 + dist_y: 12.0670 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.282000 + dist_y_std: 0.459623 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.212454 + box_width: 0.190476 + orientation: 2.63000e-07 + rcs: 4.71422 + score: 66.6667 + life_cycles: 11 + box_valid: 1 + object_status: 3 + - object_id: 28 + dist_x: 9.36614 + dist_y: 5.99689 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.135506 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.637363 + box_width: 0.256410 + orientation: 2.63000e-07 + rcs: 1.53884 + score: 100.000 + life_cycles: 36 + box_valid: 1 + object_status: 2 + - object_id: 34 + dist_x: 6.46383 + dist_y: 11.6733 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.373558 + dist_y_std: 0.146493 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.219780 + box_width: 0.336996 + orientation: 2.63000e-07 + rcs: -10.0147 + score: 100.000 + life_cycles: 5 + box_valid: 1 + object_status: 2 + - object_id: 22 + dist_x: 4.90738 + dist_y: 15.5187 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -4.20127 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 26 + dist_x: 4.35805 + dist_y: 16.3610 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 6.93698 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 31 + dist_x: 1.15360 + dist_y: 6.40889 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0970518 + dist_y_std: 0.0677531 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 1.00366 + box_width: 0.893773 + orientation: 2.63000e-07 + rcs: -6.88813 + score: 100.000 + life_cycles: 63 + box_valid: 1 + object_status: 2 + - object_id: 21 + dist_x: 6.18916 + dist_y: 13.0558 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.267350 + dist_y_std: 0.130013 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.527472 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: -7.18124 + score: 100.000 + life_cycles: 13 + box_valid: 1 + object_status: 3 + - object_id: 33 + dist_x: 20.2338 + dist_y: 5.07218 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.126350 + dist_y_std: 0.415675 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.322344 + box_width: 0.630037 + orientation: 2.63000e-07 + rcs: -4.42110 + score: 100.000 + life_cycles: 9 + box_valid: 1 + object_status: 2 + - object_id: 27 + dist_x: 31.5226 + dist_y: -2.06915 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 7.79189 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 25 + dist_x: 18.7964 + dist_y: 3.58898 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.104376 + dist_y_std: 0.357077 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.534799 + box_width: 0.688645 + orientation: 2.63000e-07 + rcs: -2.95555 + score: 93.3333 + life_cycles: 17 + box_valid: 1 + object_status: 3 + - object_id: 24 + dist_x: 3.40587 + dist_y: 15.6743 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.5276 + score: 53.3333 + life_cycles: 1 + box_valid: 0 + object_status: 0 + - object_id: 29 + dist_x: 24.5460 + dist_y: 3.90943 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0769090 + dist_y_std: 0.258194 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.307692 + box_width: 0.622711 + orientation: 2.63000e-07 + rcs: 4.29897 + score: 100.000 + life_cycles: 23 + box_valid: 1 + object_status: 2 + - object_id: 30 + dist_x: 31.0099 + dist_y: -6.34479 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 24.3283 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 36 + dist_x: 3.23192 + dist_y: 21.4698 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: -10.9184 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 41 + dist_x: -2.84737 + dist_y: 6.15254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0952206 + dist_y_std: 0.0897271 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.315018 + box_width: 0.234432 + orientation: 2.63000e-07 + rcs: 1.34343 + score: 100.000 + life_cycles: 91 + box_valid: 1 + object_status: 2 + - object_id: 39 + dist_x: 4.69680 + dist_y: 16.0039 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 30.0000 + dist_y_std: 30.0000 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.00000 + box_width: 0.00000 + orientation: 2.63000e-07 + rcs: 1.85637 + score: 26.6667 + life_cycles: 2 + box_valid: 0 + object_status: 3 + - object_id: 13 + dist_x: 29.0964 + dist_y: -3.08542 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.188610 + dist_y_std: 0.783739 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.117216 + box_width: 0.271062 + orientation: 2.63000e-07 + rcs: 13.2633 + score: 40.0000 + life_cycles: 7 + box_valid: 1 + object_status: 3 + - object_id: 43 + dist_x: 16.6631 + dist_y: 4.09254 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.0787401 + dist_y_std: 0.186779 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.358974 + box_width: 0.681319 + orientation: 2.63000e-07 + rcs: 6.39961 + score: 100.000 + life_cycles: 48 + box_valid: 1 + object_status: 2 + - object_id: 40 + dist_x: 16.9836 + dist_y: 11.8748 + v_abs_x: -1.31900e-06 + v_abs_y: -1.31900e-06 + a_abs_x: 1.92000e-07 + a_abs_y: 1.92000e-07 + dist_x_std: 0.250870 + dist_y_std: 0.351584 + v_abs_x_std: 0.00000 + v_abs_y_std: 0.00000 + a_abs_x_std: 0.00000 + a_abs_y_std: 0.00000 + box_length: 0.373626 + box_width: 0.593407 + orientation: 2.63000e-07 + rcs: -4.49438 + score: 80.0000 + life_cycles: 11 + box_valid: 1 + object_status: 3 diff --git a/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 b/nebula_tests/data/continental/srr520/1708578209/1708578209.db3 new file mode 100644 index 0000000000000000000000000000000000000000..1300ded382c73b3fad886c545ba569cfa8dc907c GIT binary patch literal 28672 zcmeI5dsI_bzQ@lqF9<FaRiEnK!QX=h)F=i2PC`^tRh;k zqD5ql$aEa#TDABXij;O7z1P)JZL7V*@^N+0>Fuq3ScS>m*?Co=ceN||V^-HW3r^12 zzq3F4yT9iohked3hgB<6y7s)hQNs&q{mzIG=jbyz{LldSa@-q^-%Th&&ncSB);l7^AwQR-> zw1&9+Rl*>hJ3;sI^+k1Cw30l9%(9u~l{8Z`_=1!~DFywr!7{=^MNR9F!l8eZtCiklzFyBE%mG+u2iCxa0Mx^3%HiDAYCX<_nI;V)yF|q zkgGAxiI)_a%6?9dQc@)2W+Y|;5mq5vRIe6kWNNuYAupF`<;6;|c|7}at>S_LnOY_l zZz|S^RqEmbxk9FS862d;F_V#5W+Sx{W5B>|?gK-0^(R(`rq)u78#xkjrlex)hn*c|12hkzeyq8y+c zpd6qapd6qapd6qapd6qapd6qapd6qa_$@iW#BpRwVM!6VRAe0ggPZ=;L^(h?Ksi7; zKsi7;Ksi7;Ksi7;Ksi7;Ksi7;@Edjj-v4K-5b#4ylmnCllmnCllmnCllmnCllmnCl zlmnCl4s@V~jna{|OWYfnAsU%dBUi2uQG*n7bx47_Sg92&WChv~r7W*RArY%2`5R?g zjd`s|vtAPdW+ACCnq~^-OVz>LLXg;9tlkpK4P$Y+QkhCy7|V?Wi)xudF3FQCLw=D&&@R&|!&2CN>H)=I#F)sHC)5E)^RS+l^lk0Fv@$;{0M|sZ41k zftCs+3XP04u{~j-pevL=8hHPoKo%n|uJnB5ELiweQz=11m^59ESMU4l`gW-><1+z{ zetJ{Zb!dcPc|3se)lw#cbnK$fSDTN?Zhm@{=65d=-N!{x{qL&s^tp9^VLdoKQ1MUW zR^6EK2?Hp;x57}-0IOMM;)KP`b8utB2Svt)FY)pkEDaCuce6I^2`U&HHkSFr%0!CK zS1#9$4HFIGL=+uB5Pa9+$LL{%Yla)Q@4GsiVG>HLa38sI2qAo0Tj~)a=7HNj1W!r7 z8;{@;=H4eFLUJ`DES)UQA$lv>C8a%27Ya){TS|6wN)!+9Gy)(8UgcGMnth;+R_WRJ zu+zEjJN*g812LSnv{0#X?F5DkKRQW_NmE>fpa&+l7Iew?%`OE zE1Rq0V#sU9m(Scl4i#!DTfn0ouyMT4*5J%Orx#DLsDlJTe_`kXkAV2aa|Ac_S8CJu zD{7B#2O%Ku)WQ^Nf9>Oukar-#Q9PQ1d34D8C|cs6{xVGd#W{+{bTE$vdCyLlIHIC9s{0F za4s5}g*4B>Y$vO80UvMve@|ASL3DpunKy@IPh zWC|n0*s!_1Nc@&zvly8|;*i(=uEeSj)7`6mHX(2KGqBHSH6K|3|a${PSQ;J`m3$_CNMPfz%l;??fz?)9iVH(`a2dQgE|b-%j7tnZ0+e>5=a3=JVj0Lg&7t|S}J zL$|kfuc-oBh7X%N1pdS^Y;Q)A=rgN(bwg%etJRJMa6E6RKw?NHftxhaf?8Xukor|H;3LxC6^FHVq`w#vry)O*z)nNVMe(eSCd+qGuiD{t>T z#`lLaC>#P@SRZp-bX;bi%H)HHr|waLRV+1 zfZTVs@RpGe4P{DX65U&;sr}Q0JQ)MCLg*lPBa|jsX6-n);iKSx#1`{dKtwffH7c5}-oRj*qNw#~Zk-xw*L*-B4AGyg^Qa zyk{+LR$hP}sr_U^UbS%tvYX^PPMX4K+tfZY&lT*CqyLT2>28o`9L!ig@8{O}=q{To zAn(v}%@xAPTZY|YB$N!STU_I63S<7x<|x7=y$srI;CObo@+>DSZ1x?borM}tR(*tV zWOZ(`s{%L;M^Sxs=``a|&NxMYUpTS>W&W@PulD)JTic8yE9~6zTPS)3jI5dusX?qC z#%t3{)C)@+lcq)8!_PbWY#7K<34IP;T%i{FOx4Ee$!P&kGqu90Kg{(l6Hfo=N{3Vk zMpol^GrafiDPYe5;_aDjp@M}3=Dg@yVvXAD0>mIamJYL-Wi@RvN zfu0=-BI3lA7iFTz^v3A3tcc^n=4?S^RrWHaAmY7igi06{*YtMFlBng1J%jA1Z=W}9 z06aS66%Myp`y0KhxuIJW>(kj(urfCI(&lR6g3;?-r68_6(*rI`B8*%}U0eNZnZM;R@PuCWpix_Q-GVsOstFklGWBM}^C_NryY=%a5>CEy9$wU#{+=rN(c6bCg^$x^P5|t=O7TUrwa{ zW$$1vp1LwQx?2eTCj{grEU@v?ioQ%9a!EUUDMv3@H7CMbie^kxbIYSfg;`#>QyxFtD{gudBkNvzQa||ai8JH{F0%2m6`#^BI<2h}`WASs%aMMs z!q(n=!OX$L{$ErnadB9AWIHY<4whu|bA2-E6sY8BxJbd59#ZTn=OYLk^1NFvS^Fz{ z?c?xA?821?!yh>n)(Lm1MRLz8^jv=7A!+kAY(v(){d!V?rAPNnDembImZ9o`+oYbp zsYe}E4Kh$%#__aH8TIwJ_Jwp`CARrkctRPr`KVjORlM|K)8Dk{=2-t3JA_+;o=mDI zx5VY_EJwGx&*4|%+gAETd=0L%a>nth!v&FDUbQ@duv}2LX#;QAyCLmv#3-}AwXs~q zufOUXpu$>{9y9dBe!t|1QBGTjUt~Y?NOFXq7F>>Xg*?RV6Kj8uRk8S8v}0ha-;26B zVk6Ge&bWWG@&JA=y*FV$Hu!OJLe7atC_txQ}N$dF`$r?+cI-G70h? zO}6p=bByga>hj47U$?8?Lv=il7Gg*}m3JP$z13sW5bO3gSVSK&oGgt#g5B+iX||_4MP$n6u#h;Wm!v=q zMfJw#mY7Nq56C;v?l{b%d?4>xu%md>Aa5Al^tar{u&=+qkY`H@u-Cuokf+*bNeZx+Hv{r)Ne1@vxR7T{ z3b2>=8syoM0_^4aL0