From ca66948584798030f5418032951b2c6d688ca4f6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 13 Dec 2023 15:42:41 +0900 Subject: [PATCH 01/42] feat: initial implementation of the continentla ars548 driver The initial implementation consists of a working hw_interface, decoder, and ros wrappers. The full ros wrappers to be integrated with other coebases have not been implemented. Since this is the first non-lidar sensor in the codebase, some structural changes were tentatively proposed. Some refactoring is still needed Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_common.hpp | 57 + .../nebula_common/hesai/hesai_common.hpp | 52 +- .../include/nebula_common/nebula_common.hpp | 62 +- .../robosense/robosense_common.hpp | 25 +- .../velodyne/velodyne_common.hpp | 20 +- nebula_decoders/CMakeLists.txt | 6 + .../decoders/continental_ars548.hpp | 173 +++ .../decoders/continental_ars548_decoder.hpp | 69 ++ .../decoders/continental_packets_decoder.hpp | 54 + nebula_decoders/package.xml | 5 +- .../decoders/continental_ars548.cpp | 175 +++ .../decoders/continental_ars548_decoder.cpp | 893 ++++++++++++++++ nebula_hw_interfaces/CMakeLists.txt | 5 + .../nebula_hw_interface_base.hpp | 22 +- ...ontinental_radar_ethernet_hw_interface.hpp | 252 +++++ .../continental_types.hpp | 148 +++ nebula_hw_interfaces/package.xml | 7 +- ...ontinental_radar_ethernet_hw_interface.cpp | 995 ++++++++++++++++++ .../continental_msgs/CMakeLists.txt | 27 + .../msg/ContinentalArs548Detection.msg | 23 + .../msg/ContinentalArs548DetectionList.msg | 13 + .../msg/ContinentalArs548Object.msg | 44 + .../msg/ContinentalArs548ObjectList.msg | 3 + .../continental_msgs/msg/NebulaPackets.msg | 2 + nebula_messages/continental_msgs/package.xml | 26 + nebula_messages/nebula_msgs/CMakeLists.txt | 24 + .../nebula_msgs/msg/NebulaPacket.msg | 2 + .../nebula_msgs/msg/NebulaPackets.msg | 2 + nebula_messages/nebula_msgs/package.xml | 25 + nebula_ros/CMakeLists.txt | 20 + .../common/nebula_driver_ros_wrapper_base.hpp | 23 +- ...tal_radar_ethernet_decoder_ros_wrapper.hpp | 113 ++ ...adar_ethernet_hw_interface_ros_wrapper.hpp | 136 +++ .../robosense_decoder_ros_wrapper.hpp | 21 +- .../robosense_hw_interface_ros_wrapper.hpp | 24 +- .../robosense_hw_monitor_ros_wrapper.hpp | 20 + .../launch/continental_launch_all_hw.xml | 34 + nebula_ros/package.xml | 5 +- ...tal_radar_ethernet_decoder_ros_wrapper.cpp | 210 ++++ ...adar_ethernet_hw_interface_ros_wrapper.cpp | 349 ++++++ .../robosense_decoder_ros_wrapper.cpp | 21 +- .../robosense_hw_interface_ros_wrapper.cpp | 18 +- .../robosense_hw_monitor_ros_wrapper.cpp | 16 + 43 files changed, 4174 insertions(+), 47 deletions(-) create mode 100644 nebula_common/include/nebula_common/continental/continental_common.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp create mode 100644 nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp create mode 100644 nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp create mode 100644 nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp create mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp create mode 100644 nebula_messages/continental_msgs/CMakeLists.txt create mode 100644 nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalArs548DetectionList.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg create mode 100644 nebula_messages/continental_msgs/msg/ContinentalArs548ObjectList.msg create mode 100644 nebula_messages/continental_msgs/msg/NebulaPackets.msg create mode 100644 nebula_messages/continental_msgs/package.xml create mode 100644 nebula_messages/nebula_msgs/CMakeLists.txt create mode 100644 nebula_messages/nebula_msgs/msg/NebulaPacket.msg create mode 100644 nebula_messages/nebula_msgs/msg/NebulaPackets.msg create mode 100644 nebula_messages/nebula_msgs/package.xml create mode 100644 nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/launch/continental_launch_all_hw.xml create mode 100644 nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp new file mode 100644 index 000000000..3647bfeac --- /dev/null +++ b/nebula_common/include/nebula_common/continental/continental_common.hpp @@ -0,0 +1,57 @@ + +// Copyright 2023 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_COMMON_H +#define NEBULA_CONTINENTAL_COMMON_H + +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" + +#include +#include +#include +#include +#include +#include +namespace nebula +{ +namespace drivers +{ +/// @brief struct for Hesai sensor configuration +struct ContinentalRadarEthernetSensorConfiguration : EthernetSensorConfigurationBase +{ + std::string multicast_ip{}; + uint16_t configuration_host_port{}; + uint16_t configuration_sensor_port{}; +}; + +/// @brief Convert ContinentalRadarEthernetSensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, ContinentalRadarEthernetSensorConfiguration const & arg) +{ + os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip + << ", ConfigurationHostPort: " << arg.configuration_host_port + << ", ConfigurationSensorPort: " << arg.configuration_sensor_port; + return os; +} + +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONTINENTAL_COMMON_H diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index a19b025b0..028a71f2d 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 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_HESAI_COMMON_H #define NEBULA_HESAI_COMMON_H @@ -8,13 +22,16 @@ #include #include #include +#include #include +#include +#include namespace nebula { namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : SensorConfigurationBase +struct HesaiSensorConfiguration : LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -29,7 +46,7 @@ struct HesaiSensorConfiguration : SensorConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration const & arg) { - os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle << ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold; @@ -91,9 +108,10 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase line_ss >> laser_id >> sep >> elevation >> sep >> azimuth; elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; -// std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl; + // std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << + // azimuth << std::endl; } -// std::cout << "LoadFromString fin" << std::endl; + // std::cout << "LoadFromString fin" << std::endl; return Status::OK; } @@ -122,14 +140,15 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString( + const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { return Status::CANNOT_SAVE_FILE; } ofs << calibration_string; -// std::cout << calibration_string << std::endl; + // std::cout << calibration_string << std::endl; ofs.close(); return Status::OK; @@ -162,9 +181,8 @@ struct HesaiCorrection inline nebula::Status LoadFromBinary(const std::vector & buf) { size_t index; - for (index = 0; index < buf.size()-1; index++) { - if(buf[index]==0xee && buf[index+1]==0xff) - break; + for (index = 0; index < buf.size() - 1; index++) { + if (buf[index] == 0xee && buf[index + 1] == 0xff) break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -296,7 +314,7 @@ struct HesaiCorrection // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read((char *)&c, sizeof(unsigned char)); + ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } LoadFromBinary(buf); @@ -309,7 +327,8 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveFileFromBinary( + const std::string & correction_file, const std::vector & buf) { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -319,21 +338,20 @@ struct HesaiCorrection } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto &byte : buf) { + for (const auto & byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if(sop_received) { + if (sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if(sop_received) - return Status::OK; + if (sop_received) return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -457,8 +475,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARAT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST - || return_mode == ReturnMode::DUAL) return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) + return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index c75359aa8..ca0b99adb 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 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_COMMON_H #define NEBULA_COMMON_H @@ -327,6 +341,7 @@ enum class SensorModel { ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, + CONTINENTAL_ARS548 }; /// @brief not used? @@ -417,6 +432,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_BPEARL_V4: os << "BPEARL V4.0"; break; + case SensorModel::CONTINENTAL_ARS548: + os << "ARS548"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -428,11 +446,21 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel struct SensorConfigurationBase { SensorModel sensor_model; - ReturnMode return_mode; + std::string frame_id; +}; + +/// @brief Base struct for Ethernet-based Sensor configuration +struct EthernetSensorConfigurationBase : SensorConfigurationBase +{ std::string host_ip; std::string sensor_ip; - std::string frame_id; uint16_t data_port; +}; + +/// @brief Base struct for Lidar configuration +struct LidarConfigurationBase : EthernetSensorConfigurationBase +{ + ReturnMode return_mode; uint16_t frequency_ms; uint16_t packet_mtu_size; CoordinateMode coordinate_mode; @@ -447,12 +475,31 @@ struct SensorConfigurationBase /// @param os /// @param arg /// @return stream +inline std::ostream & operator<<(std::ostream & os, SensorConfigurationBase const & arg) +{ + os << "SensorModel: " << arg.sensor_model << ", FrameID: " << arg.frame_id; + return os; +} + +/// @brief Convert EthernetSensorConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<(std::ostream & os, EthernetSensorConfigurationBase const & arg) +{ + os << (SensorConfigurationBase)(arg) << ", HostIP: " << arg.host_ip + << ", SensorIP: " << arg.sensor_ip << ", DataPort: " << arg.data_port; + return os; +} + +/// @brief Convert LidarConfigurationBase to string (Overloading the << operator) +/// @param os +/// @param arg +/// @return stream inline std::ostream & operator<<( - std::ostream & os, nebula::drivers::SensorConfigurationBase const & arg) + std::ostream & os, nebula::drivers::LidarConfigurationBase const & arg) { - os << "SensorModel: " << arg.sensor_model << ", ReturnMode: " << arg.return_mode - << ", HostIP: " << arg.host_ip << ", SensorIP: " << arg.sensor_ip - << ", FrameID: " << arg.frame_id << ", DataPort: " << arg.data_port + os << (EthernetSensorConfigurationBase)(arg) << ", ReturnMode: " << arg.return_mode << ", Frequency: " << arg.frequency_ms << ", MTU: " << arg.packet_mtu_size << ", Use sensor time: " << arg.use_sensor_time; return os; @@ -491,6 +538,7 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; + if (sensor_model == "ARS548") return SensorModel::CONTINENTAL_ARS548; return SensorModel::UNKNOWN; } @@ -538,6 +586,8 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + case SensorModel::CONTINENTAL_ARS548: + return "ARS548"; default: return "UNKNOWN"; } diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index e92302f2d..8a09a5978 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 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" @@ -9,6 +23,7 @@ #include #include #include +#include #include namespace nebula @@ -20,7 +35,7 @@ namespace drivers constexpr uint8_t BPEARL_V4_FLAG = 0x04; /// @brief struct for Robosense sensor configuration -struct RobosenseSensorConfiguration : SensorConfigurationBase +struct RobosenseSensorConfiguration : LidarConfigurationBase { uint16_t gnss_port{}; // difop double scan_phase{}; // start/end angle @@ -36,7 +51,7 @@ struct RobosenseSensorConfiguration : SensorConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg) { - os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; return os; @@ -188,10 +203,10 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase void CreateCorrectedChannels() { - for(auto& correction : calibration) { + for (auto & correction : calibration) { uint16_t channel = 0; - for(const auto& compare:calibration) { - if(compare.elevation < correction.elevation) ++channel; + for (const auto & compare : calibration) { + if (compare.elevation < correction.elevation) ++channel; } correction.channel = channel; } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index fd963cd65..b8c6fb5f1 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 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_VELODYNE_COMMON_H #define NEBULA_VELODYNE_COMMON_H @@ -7,12 +21,14 @@ #include #include +#include + namespace nebula { namespace drivers { /// @brief struct for Velodyne sensor configuration -struct VelodyneSensorConfiguration : SensorConfigurationBase +struct VelodyneSensorConfiguration : LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -26,7 +42,7 @@ struct VelodyneSensorConfiguration : SensorConfigurationBase /// @return stream inline std::ostream & operator<<(std::ostream & os, VelodyneSensorConfiguration const & arg) { - os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port + os << (LidarConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; return os; diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 6f38b97af..11485489b 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -51,6 +51,12 @@ ament_auto_add_library(nebula_decoders_robosense_info SHARED src/nebula_decoders_robosense/robosense_info_driver.cpp ) +# Continental +ament_auto_add_library(nebula_decoders_continental SHARED + src/nebula_decoders_continental/decoders/continental_ars548.cpp + src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp new file mode 100644 index 000000000..f98f3ee20 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp @@ -0,0 +1,173 @@ +// Copyright 2023 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 +/** + * Continental ARS548 + */ +#include "continental_msgs/msg/continental_ars548_detection_list.hpp" +#include "continental_msgs/msg/continental_ars548_object_list.hpp" +#include "radar_msgs/msg/radar_scan.hpp" +#include "radar_msgs/msg/radar_tracks.hpp" + +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +constexpr int SERVICE_ID_BYTE = 0; +constexpr int METHOD_ID_BYTE = 2; +constexpr int LENGTH_BYTE = 4; + +constexpr int DETECTION_LIST_METHOD_ID = 336; +constexpr int OBJECT_LIST_METHOD_ID = 329; + +constexpr int DETECTION_LIST_UDP_PAYPLOAD = 35336; +constexpr int OBJECT_LIST_UDP_PAYPLOAD = 9401; + +constexpr int DETECTION_LIST_PDU_LENGTH = 35328; +constexpr int OBJECT_LIST_PDU_LENGTH = 9393; + +constexpr int DETECTION_LIST_CRC_BYTE = 16; +constexpr int DETECTION_LIST_LENGTH_BYTE = 24; +constexpr int DETECTION_LIST_SQC_BYTE = 28; +constexpr int DETECTION_LIST_DATA_ID_BYTE = 32; +constexpr int DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; +constexpr int DETECTION_LIST_TIMESTAMP_SECONDS_BYTE = 40; +constexpr int DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; +constexpr int DETECTION_LIST_ORIGIN_X_POS_BYTE = 52; +constexpr int DETECTION_LIST_ORIGIN_Y_POS_BYTE = 60; +constexpr int DETECTION_LIST_ORIGIN_Z_POS_BYTE = 68; +constexpr int DETECTION_LIST_PITCH_BYTE = 84; +constexpr int DETECTION_LIST_PITCH_STD_BYTE = 88; +constexpr int DETECTION_LIST_YAW_BYTE = 92; +constexpr int DETECTION_LIST_YAW_STD_BYTE = 96; +constexpr int DETECTION_LIST_ARRAY_BYTE = 101; +constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE = 35301; +constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE = 35305; +constexpr int DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE = 35309; +constexpr int DETECTION_LIST_AZIMUTH_CORRECTION_BYTE = 35313; +constexpr int DETECTION_LIST_ELEVATION_CORRECTION_BYTE = 35317; +constexpr int DETECTION_LIST_ALIGNMENT_STATUS_BYTE = 35321; + +constexpr int DETECTION_STRUCT_SIZE = 44; +constexpr int DETECTION_AZIMUTH_ANGLE_BYTE = 0; +constexpr int DETECTION_AZIMUTH_ANGLE_STD_BYTE = 4; +constexpr int DETECTION_INVALID_FLAGS_BYTE = 8; +constexpr int DETECTION_ELEVATION_ANGLE_BYTE = 9; +constexpr int DETECTION_ELEVATION_ANGLE_STD_BYTE = 13; +constexpr int DETECTION_RANGE_BYTE = 17; +constexpr int DETECTION_RANGE_STD_BYTE = 21; +constexpr int DETECTION_RANGE_RATE_BYTE = 25; +constexpr int DETECTION_RANGE_RATE_STD_BYTE = 29; +constexpr int DETECTION_RCS_BYTE = 33; +constexpr int DETECTION_MEASUREMENT_ID_BYTE = 34; +constexpr int DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = 36; +constexpr int DETECTION_CLASSIFICATION_BYTE = 37; +constexpr int DETECTION_MULT_TARGET_PROBABILITY_BYTE = 38; +constexpr int DETECTION_OBJECT_ID_BYTE = 39; +constexpr int DETECTION_AMBIGUITY_FLAG_BYTE = 41; + +constexpr int OBJECT_LIST_CRC_BYTE = 16; +constexpr int OBJECT_LIST_LENGTH_BYTE = 24; +constexpr int OBJECT_LIST_SQC_BYTE = 28; +constexpr int OBJECT_LIST_DATA_ID_BYTE = 32; +constexpr int OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; +constexpr int OBJECT_LIST_TIMESTAMP_SECONDS_BYTE = 40; +constexpr int OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; +constexpr int OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE = 50; +constexpr int OBJECT_LIST_ARRAY_BYTE = 51; + +constexpr int OBJECT_STRUCT_SIZE = 187; +constexpr int OBJECT_STATUS_SENSOR_BYTE = 0; +constexpr int OBJECT_ID_BYTE = 2; +constexpr int OBJECT_AGE_BYTE = 6; +constexpr int OBJECT_STATUS_MEASUREMENT_BYTE = 8; +constexpr int OBJECT_STATUS_MOVEMENT_BYTE = 9; +constexpr int OBJECT_POSITION_REFERENCE_BYTE = 12; +constexpr int OBJECT_POSITION_X_BYTE = 13; +constexpr int OBJECT_POSITION_X_STD_BYTE = 17; +constexpr int OBJECT_POSITION_Y_BYTE = 21; +constexpr int OBJECT_POSITION_Y_STD_BYTE = 25; +constexpr int OBJECT_POSITION_Z_BYTE = 29; +constexpr int OBJECT_POSITION_Z_STD_BYTE = 33; + +constexpr int OBJECT_POSITION_COVARIANCE_XY_BYTE = 37; +constexpr int OBJECT_POSITION_ORIENTATION_BYTE = 41; +constexpr int OBJECT_POSITION_ORIENTATION_STD_BYTE = 45; + +constexpr int OBJECT_EXISTENCE_PROBABILITY_BYTE = 50; + +constexpr int OBJECT_CLASSIFICATION_CAR_BYTE = 58; +constexpr int OBJECT_CLASSIFICATION_TRUCK_BYTE = 59; +constexpr int OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = 60; +constexpr int OBJECT_CLASSIFICATION_BICYCLE_BYTE = 61; +constexpr int OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = 62; +constexpr int OBJECT_CLASSIFICATION_ANIMAL_BYTE = 63; +constexpr int OBJECT_CLASSIFICATION_HAZARD_BYTE = 64; +constexpr int OBJECT_CLASSIFICATION_UNKNOWN_BYTE = 65; + +constexpr int OBJECT_DYNAMICS_ABS_VEL_X_BYTE = 69; +constexpr int OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = 73; +constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = 77; +constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = 81; +constexpr int OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = 85; + +constexpr int OBJECT_DYNAMICS_REL_VEL_X_BYTE = 90; +constexpr int OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = 94; +constexpr int OBJECT_DYNAMICS_REL_VEL_Y_BYTE = 98; +constexpr int OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = 102; +constexpr int OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = 106; + +constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = 111; +constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = 115; +constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = 119; +constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = 123; +constexpr int OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = 127; + +constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = 132; +constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = 136; +constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = 140; +constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = 144; +constexpr int OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = 148; + +constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = 153; +constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = 157; + +constexpr int OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = 166; +constexpr int OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = 179; + +pcl::PointCloud::Ptr convertToPointcloud( + const continental_msgs::msg::ContinentalArs548DetectionList & msg); + +pcl::PointCloud::Ptr convertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); + +radar_msgs::msg::RadarScan convertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & msg); + +radar_msgs::msg::RadarTracks convertToRadarTracks( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); + +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula 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 new file mode 100644 index 000000000..c7d9e394a --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -0,0 +1,69 @@ +// Copyright 2023 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_ars548.hpp" +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" +#include +#include + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +/// @brief Continental Radar decoder (ARS548) +class ContinentalARS548Decoder : public ContinentalPacketsDecoder +{ +public: + /// @brief Constructor + /// @param sensor_configuration SensorConfiguration for this decoder + explicit ContinentalARS548Decoder( + const std::shared_ptr & + sensor_configuration); + + /// @brief Function for parsing NebulaPackets + /// @param nebula_packets + /// @return Resulting flag + bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override; + + bool ParseDetectionsListPacket(const std::vector & data); + bool ParseObjectsListPacket(const std::vector & data); + + Status RegisterDetectionListCallback( + std::function)> + detection_list_callback); + Status RegisterObjectListCallback( + std::function)> + object_list_callback); + +private: + std::function msg)> + detection_list_callback_; + std::function msg)> + object_list_callback_; +}; + +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula 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 new file mode 100644 index 000000000..f7867aeb1 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 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_WS_CONTINENTAL_PACKETS_DECODER_HPP +#define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP + +#include "nebula_common/continental/continental_common.hpp" +#include "nebula_common/point_types.hpp" + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ +/// @brief Base class for Continental Radar decoder +class ContinentalPacketsDecoder +{ +protected: + /// @brief SensorConfiguration for this decoder + std::shared_ptr sensor_configuration_; + +public: + ContinentalPacketsDecoder(ContinentalPacketsDecoder && c) = delete; + ContinentalPacketsDecoder & operator=(ContinentalPacketsDecoder && c) = delete; + ContinentalPacketsDecoder(const ContinentalPacketsDecoder & c) = delete; + ContinentalPacketsDecoder & operator=(const ContinentalPacketsDecoder & c) = delete; + + virtual ~ContinentalPacketsDecoder() = default; + ContinentalPacketsDecoder() = default; + + /// @brief Virtual function for parsing NebulaPackets based on packet structure + /// @param nebula_packets + /// @return Resulting flag + virtual bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) = 0; +}; +} // namespace drivers +} // namespace nebula +#endif // NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index 6956bc3d1..0751cbe7f 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -13,14 +13,17 @@ ros_environment angles + continental_msgs libpcl-all-dev nebula_common + nebula_msgs pandar_msgs pcl_conversions + radar_msgs + robosense_msgs sensor_msgs velodyne_msgs yaml-cpp - robosense_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp new file mode 100644 index 000000000..88b1e7f82 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp @@ -0,0 +1,175 @@ +// Copyright 2023 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_ars548.hpp" + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ + +pcl::PointCloud::Ptr convertToPointcloud( + const continental_msgs::msg::ContinentalArs548DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + pcl::PointXYZ point{}; + for (const auto & detection : msg.detections) { + point.x = + std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; + point.y = + std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; + point.z = std::sin(detection.elevation_angle) * detection.range; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr convertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + pcl::PointXYZ point{}; + for (const auto & detection : msg.objects) { + point.x = static_cast(detection.position.x); + point.y = static_cast(detection.position.y); + point.z = static_cast(detection.position.z); + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan convertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & 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.invalid_azimuth || detection.invalid_distance || detection.invalid_elevation || + detection.invalid_range_rate) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = detection.elevation_angle; + 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 convertToRadarTracks( + const continental_msgs::msg::ContinentalArs548ObjectList & 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 int16_t CAR_ID = 32001; + constexpr int16_t TRUCK_ID = 32002; + constexpr int16_t MOTORCYCLE_ID = 32005; + constexpr int16_t BICYCLE_ID = 32006; + constexpr int16_t PEDESTRIAN_ID = 32007; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & detection : msg.objects) { + track_msg.uuid.uuid[0] = static_cast(detection.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((detection.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((detection.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((detection.object_id >> 24) & 0xff); + track_msg.position = detection.position; + track_msg.velocity = detection.absolute_velocity; + track_msg.acceleration = detection.absolute_acceleration; + track_msg.size.x = detection.shape_length_edge_mean; + track_msg.size.y = detection.shape_width_edge_mean; + track_msg.size.z = 1.f; + + uint8_t max_score = detection.classification_unknown; + track_msg.classification = UNKNOWN_ID; + + if (detection.classification_car > max_score) { + max_score = detection.classification_car; + track_msg.classification = CAR_ID; + } + if (detection.classification_truck > max_score) { + max_score = detection.classification_truck; + track_msg.classification = TRUCK_ID; + } + if (detection.classification_motorcycle > max_score) { + max_score = detection.classification_motorcycle; + track_msg.classification = MOTORCYCLE_ID; + } + if (detection.classification_bicycle > max_score) { + max_score = detection.classification_bicycle; + track_msg.classification = BICYCLE_ID; + } + if (detection.classification_pedestrian > max_score) { + max_score = detection.classification_pedestrian; + track_msg.classification = PEDESTRIAN_ID; + } + + track_msg.position_covariance[0] = static_cast(detection.position_std.x); + track_msg.position_covariance[1] = detection.position_covariance_xy; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = static_cast(detection.position_std.y); + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = static_cast(detection.position_std.z); + + track_msg.velocity_covariance[0] = static_cast(detection.absolute_velocity_std.x); + track_msg.velocity_covariance[1] = detection.absolute_velocity_covariance_xy; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = static_cast(detection.absolute_velocity_std.y); + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = static_cast(detection.absolute_velocity_std.z); + + track_msg.acceleration_covariance[0] = + static_cast(detection.absolute_acceleration_std.x); + track_msg.acceleration_covariance[1] = detection.absolute_acceleration_covariance_xy; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = + static_cast(detection.absolute_acceleration_std.y); + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = + static_cast(detection.absolute_acceleration_std.z); + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +} // namespace continental_ars548 +} // 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 new file mode 100644 index 000000000..0e3e2fe70 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -0,0 +1,893 @@ +// Copyright 2023 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_ars548_decoder.hpp" + +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp" + +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +ContinentalARS548Decoder::ContinentalARS548Decoder( + const std::shared_ptr & + sensor_configuration) +{ + sensor_configuration_ = sensor_configuration; +} + +Status ContinentalARS548Decoder::RegisterDetectionListCallback( + std::function)> + detection_list_callback) +{ + detection_list_callback_ = std::move(detection_list_callback); + return Status::OK; +} + +Status ContinentalARS548Decoder::RegisterObjectListCallback( + std::function)> + object_list_callback) +{ + object_list_callback_ = std::move(object_list_callback); + return Status::OK; +} + +bool ContinentalARS548Decoder::ProcessPackets( + const nebula_msgs::msg::NebulaPackets & nebula_packets) +{ + if (nebula_packets.packets.size() != 1) { + return false; + } + + const auto & data = nebula_packets.packets[0].data; + + if (data.size() < LENGTH_BYTE + sizeof(uint32_t)) { + return false; + } + + const uint16_t service_id = + (static_cast(data[SERVICE_ID_BYTE]) << 8) | data[SERVICE_ID_BYTE + 1]; + const uint16_t method_id = + (static_cast(data[METHOD_ID_BYTE]) << 8) | data[METHOD_ID_BYTE + 1]; + const uint32_t length = (static_cast(data[LENGTH_BYTE]) << 24) | + (static_cast(data[LENGTH_BYTE + 1]) << 16) | + (static_cast(data[LENGTH_BYTE + 2]) << 8) | + static_cast(data[LENGTH_BYTE + 3]); + + if (service_id != 0) { + return false; + } + + if (method_id == DETECTION_LIST_METHOD_ID) { + if (data.size() != DETECTION_LIST_UDP_PAYPLOAD || length != DETECTION_LIST_PDU_LENGTH) { + return false; + } + + return ParseDetectionsListPacket(data); + } else if (method_id == OBJECT_LIST_METHOD_ID) { + if (data.size() != OBJECT_LIST_UDP_PAYPLOAD || length != OBJECT_LIST_PDU_LENGTH) { + return false; + } + + return ParseObjectsListPacket(data); + } + + return true; +} + +bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector & data) +{ + auto msg_ptr = std::make_unique(); + auto & msg = *msg_ptr; + + msg.header.frame_id = sensor_configuration_->frame_id; + msg.header.stamp.nanosec = + (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | + data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; + msg.header.stamp.sec = + (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | + data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 3]; + msg.stamp_sync_status = data[DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); + + const uint32_t origin_pos_x_u = + (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 2]) << 8) | + data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 3]; + const uint32_t origin_pos_y_u = + (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 2]) << 8) | + data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 3]; + const uint32_t origin_pos_z_u = + (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 2]) << 8) | + data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 3]; + + float origin_pos_x_f, origin_pos_y_f, origin_pos_z_f; + + std::memcpy(&origin_pos_x_f, &origin_pos_x_u, sizeof(origin_pos_x_u)); + std::memcpy(&origin_pos_y_f, &origin_pos_y_u, sizeof(origin_pos_y_u)); + std::memcpy(&origin_pos_z_f, &origin_pos_z_u, sizeof(origin_pos_z_u)); + + msg.origin_pos.x = static_cast(origin_pos_x_f); + msg.origin_pos.y = static_cast(origin_pos_y_f); + msg.origin_pos.z = static_cast(origin_pos_z_f); + + const uint32_t origin_pitch_u = + (static_cast(data[DETECTION_LIST_PITCH_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_PITCH_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_PITCH_BYTE + 2]) << 8) | + data[DETECTION_LIST_PITCH_BYTE + 3]; + const uint32_t origin_pitch_std_u = + (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 2]) << 8) | + data[DETECTION_LIST_PITCH_STD_BYTE + 3]; + const uint32_t origin_yaw_u = (static_cast(data[DETECTION_LIST_YAW_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_YAW_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_YAW_BYTE + 2]) << 8) | + data[DETECTION_LIST_YAW_BYTE + 3]; + const uint32_t origin_yaw_std_u = + (static_cast(data[DETECTION_LIST_YAW_STD_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 2]) << 8) | + data[DETECTION_LIST_YAW_STD_BYTE + 3]; + + std::memcpy(&msg.origin_pitch, &origin_pitch_u, sizeof(origin_pitch_u)); + std::memcpy(&msg.origin_pitch_std, &origin_pitch_std_u, sizeof(origin_pitch_std_u)); + std::memcpy(&msg.origin_yaw, &origin_yaw_u, sizeof(origin_yaw_u)); + std::memcpy(&msg.origin_yaw_std, &origin_yaw_std_u, sizeof(origin_yaw_std_u)); + + const uint32_t ambiguity_free_velocity_min_u = + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 2]) << 8) | + data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 3]; + const uint32_t ambiguity_free_velocity_max_u = + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 2]) << 8) | + data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 3]; + + std::memcpy( + &msg.ambiguity_free_velocity_min, &ambiguity_free_velocity_min_u, + sizeof(ambiguity_free_velocity_min_u)); + std::memcpy( + &msg.ambiguity_free_velocity_max, &ambiguity_free_velocity_max_u, + sizeof(ambiguity_free_velocity_max_u)); + + const uint32_t alignment_azimuth_correction_u = + (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 2]) << 8) | + data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 3]; + const uint32_t alignment_elevation_correction_u = + (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 2]) << 8) | + data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 3]; + + std::memcpy( + &msg.alignment_azimuth_correction, &alignment_azimuth_correction_u, + sizeof(alignment_azimuth_correction_u)); + std::memcpy( + &msg.alignment_elevation_correction, &alignment_elevation_correction_u, + sizeof(alignment_elevation_correction_u)); + + msg.alignment_status = data[DETECTION_LIST_ALIGNMENT_STATUS_BYTE]; + + const uint32_t numer_of_detections = + (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE]) << 24) | + (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 1]) << 16) | + (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 2]) << 8) | + data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 3]; + + assert(origin_pos_x_f >= -10.f && origin_pos_x_f <= 10.f); + assert(origin_pos_y_f >= -10.f && origin_pos_y_f <= 10.f); + assert(origin_pos_z_f >= -10.f && origin_pos_z_f <= 10.f); + assert(msg.origin_pitch >= -M_PI && msg.origin_pitch <= M_PI); + assert(msg.origin_yaw >= -M_PI && msg.origin_yaw <= M_PI); + assert(msg.ambiguity_free_velocity_min >= -100.f && msg.ambiguity_free_velocity_min <= 100.f); + assert(msg.ambiguity_free_velocity_max >= -100.f && msg.ambiguity_free_velocity_max <= 100.f); + assert(numer_of_detections <= 800); + assert(msg.alignment_azimuth_correction >= -M_PI && msg.alignment_azimuth_correction <= M_PI); + assert(msg.alignment_elevation_correction >= -M_PI && msg.alignment_elevation_correction <= M_PI); + + msg.detections.resize(numer_of_detections); + + for (std::size_t detection_index = 0; detection_index < numer_of_detections; detection_index++) { + auto & detection_msg = msg.detections[detection_index]; + + const int CURRENT_DETECTION_START_BYTE = + DETECTION_LIST_ARRAY_BYTE + detection_index * DETECTION_STRUCT_SIZE; + const int CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_BYTE; + const int CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_STD_BYTE; + const int CURRENT_DETECTION_INVALID_FLAGS_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_INVALID_FLAGS_BYTE; + const int CURRENT_DETECTION_ELEVATION_ANGLE_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_BYTE; + const int CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_STD_BYTE; + const int CURRENT_DETECTION_RANGE_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_BYTE; + const int CURRENT_DETECTION_RANGE_STD_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_STD_BYTE; + const int CURRENT_DETECTION_RANGE_RATE_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_BYTE; + const int CURRENT_DETECTION_RANGE_RATE_STD_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_STD_BYTE; + const int CURRENT_DETECTION_RCS_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RCS_BYTE; + const int CURRENT_DETECTION_MEASUREMENT_ID_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_MEASUREMENT_ID_BYTE; + const int CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE; + const int CURRENT_DETECTION_CLASSIFICATION_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_CLASSIFICATION_BYTE; + const int CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_MULT_TARGET_PROBABILITY_BYTE; + const int CURRENT_DETECTION_OBJECT_ID_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_OBJECT_ID_BYTE; + const int CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE = + CURRENT_DETECTION_START_BYTE + DETECTION_AMBIGUITY_FLAG_BYTE; + + const uint32_t azimuth_angle_u = + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 3]; + const uint32_t azimuth_angle_std_u = + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 3]; + const uint8_t invalid_flags_u = + (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 3]; + const uint32_t elevation_angle_u = + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 3]; + const uint32_t elevation_angle_std_u = + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 3]; + const uint32_t range_u = (static_cast(data[CURRENT_DETECTION_RANGE_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_RANGE_BYTE + 3]; + const uint32_t range_std_u = + (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_RANGE_STD_BYTE + 3]; + const uint32_t range_rate_u = + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_RANGE_RATE_BYTE + 3]; + const uint32_t range_rate_std_u = + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 2]) << 8) | + data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 3]; + const uint8_t rcs_u = data[CURRENT_DETECTION_RCS_BYTE]; + const uint16_t measurement_id_u = + (static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE]) << 8) | + static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE + 1]); + const uint8_t positive_predictive_value_u = + data[CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE]; + const uint8_t classification_u = data[CURRENT_DETECTION_CLASSIFICATION_BYTE]; + const uint8_t multi_target_probabilty_u = data[CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE]; + const uint16_t object_id_u = + (static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE]) << 8) | + static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE + 1]); + const uint8_t ambiguity_flag_u = data[CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE]; + + assert(positive_predictive_value_u <= 100); + assert(classification_u <= 4 || classification_u == 255); + assert(multi_target_probabilty_u <= 100); + assert(ambiguity_flag_u <= 100); + + detection_msg.invalid_distance = invalid_flags_u & 0x01; + detection_msg.invalid_distance_std = invalid_flags_u & 0x01; + detection_msg.invalid_azimuth = invalid_flags_u & 0x04; + detection_msg.invalid_azimuth_std = invalid_flags_u & 0x08; + detection_msg.invalid_elevation = invalid_flags_u & 0x10; + detection_msg.invalid_elevation_std = invalid_flags_u & 0x20; + detection_msg.invalid_range_rate = invalid_flags_u & 0x40; + detection_msg.invalid_range_rate_std = invalid_flags_u & 0x01; + detection_msg.rcs = static_cast(rcs_u); + detection_msg.measurement_id = measurement_id_u; + detection_msg.positive_predictive_value = positive_predictive_value_u; + detection_msg.classification = classification_u; + detection_msg.multi_target_probabilty = multi_target_probabilty_u; + detection_msg.object_id = object_id_u; + detection_msg.ambiguity_flag = ambiguity_flag_u; + std::memcpy(&detection_msg.azimuth_angle, &azimuth_angle_u, sizeof(azimuth_angle_u)); + std::memcpy( + &detection_msg.azimuth_angle_std, &azimuth_angle_std_u, sizeof(azimuth_angle_std_u)); + std::memcpy(&detection_msg.elevation_angle, &elevation_angle_u, sizeof(elevation_angle_u)); + std::memcpy( + &detection_msg.elevation_angle_std, &elevation_angle_std_u, sizeof(elevation_angle_std_u)); + std::memcpy(&detection_msg.range, &range_u, sizeof(range_u)); + std::memcpy(&detection_msg.range_std, &range_std_u, sizeof(range_std_u)); + std::memcpy(&detection_msg.range_rate, &range_rate_u, sizeof(range_rate_u)); + std::memcpy(&detection_msg.range_rate_std, &range_rate_std_u, sizeof(range_rate_std_u)); + + assert(detection_msg.azimuth_angle >= -M_PI && detection_msg.azimuth_angle <= M_PI); + assert(detection_msg.azimuth_angle_std >= 0.f && detection_msg.azimuth_angle_std <= 1.f); + assert(detection_msg.elevation_angle >= -M_PI && detection_msg.elevation_angle <= M_PI); + assert(detection_msg.elevation_angle_std >= 0.f && detection_msg.elevation_angle_std <= 1.f); + + assert(detection_msg.range >= 0.f && detection_msg.range <= 1500.f); + assert(detection_msg.range_std >= 0.f && detection_msg.range_std <= 1.f); + assert(detection_msg.range_rate_std >= 0.f && detection_msg.range_rate_std <= 1.f); + } + + detection_list_callback_(std::move(msg_ptr)); + + return true; +} + +bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector & data) +{ + auto msg_ptr = std::make_unique(); + auto & msg = *msg_ptr; + msg.header.frame_id = sensor_configuration_->frame_id; + msg.header.stamp.nanosec = + (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | + (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | + (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | + data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; + msg.header.stamp.sec = + (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | + (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | + (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | + data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 3]; + msg.stamp_sync_status = data[OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); + + const uint8_t numer_of_objects = data[OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE]; + + msg.objects.resize(numer_of_objects); + + for (std::size_t object_index = 0; object_index < numer_of_objects; object_index++) { + auto & object_msg = msg.objects[object_index]; + + const int CURRENT_OBJECT_START_BYTE = + OBJECT_LIST_ARRAY_BYTE + object_index * OBJECT_STRUCT_SIZE; + const int CURRENT_OBJECT_ID_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ID_BYTE; + const int CURRENT_OBJECT_AGE_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_AGE_BYTE; + + const int CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MEASUREMENT_BYTE; + const int CURRENT_OBJECT_STATUS_MOVEMENT_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MOVEMENT_BYTE; + const int CURRENT_OBJECT_POSITION_REFERENCE_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_REFERENCE_BYTE; + const int CURRENT_OBJECT_POSITION_X_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_BYTE; + const int CURRENT_OBJECT_POSITION_X_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_STD_BYTE; + const int CURRENT_OBJECT_POSITION_Y_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_BYTE; + const int CURRENT_OBJECT_POSITION_Y_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_STD_BYTE; + const int CURRENT_OBJECT_POSITION_Z_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_BYTE; + const int CURRENT_OBJECT_POSITION_Z_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_STD_BYTE; + + const int CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_COVARIANCE_XY_BYTE; + const int CURRENT_OBJECT_POSITION_ORIENTATION_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_ORIENTATION_BYTE; + const int CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_ORIENTATION_STD_BYTE; + + const int CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_EXISTENCE_PROBABILITY_BYTE; + + const int CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_CAR_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_TRUCK_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_BICYCLE_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_ANIMAL_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_HAZARD_BYTE; + const int CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_UNKNOWN_BYTE; + + const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE; + + const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE; + + const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE; + + const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE; + const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE; + + const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE; + const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE; + + const int CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE; + const int CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE; + + const uint32_t object_id_u = (static_cast(data[CURRENT_OBJECT_ID_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_ID_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_ID_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_ID_BYTE + 3]; + const uint16_t object_age_u = (static_cast(data[CURRENT_OBJECT_AGE_BYTE]) << 8) | + static_cast(data[CURRENT_OBJECT_AGE_BYTE + 1]); + const uint8_t status_measurement_u = data[CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE]; + const uint8_t status_movement_u = data[CURRENT_OBJECT_STATUS_MOVEMENT_BYTE]; + const uint8_t position_reference_u = data[CURRENT_OBJECT_POSITION_REFERENCE_BYTE]; + + const uint32_t position_x_u = + (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_X_BYTE + 3]; + const uint32_t position_x_std_u = + (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 3]; + const uint32_t position_y_u = + (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_Y_BYTE + 3]; + const uint32_t position_y_std_u = + (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 3]; + const uint32_t position_z_u = + (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_Z_BYTE + 3]; + const uint32_t position_z_std_u = + (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 3]; + const uint32_t position_xy_covariance_u = + (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 3]; + const uint32_t position_orientation_u = + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 3]; + const uint32_t position_orientation_std_u = + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 3]; + const uint32_t existence_probability_u = + (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 3]; + + float position_x_f; + float position_x_std_f; + float position_y_f; + float position_y_std_f; + float position_z_f; + float position_z_std_f; + float position_xy_covariance_f; + float position_orientation_f; + float position_orientation_std_f; + float existence_probability_f; + + std::memcpy(&position_x_f, &position_x_u, sizeof(position_x_u)); + std::memcpy(&position_x_std_f, &position_x_std_u, sizeof(position_x_std_u)); + std::memcpy(&position_y_f, &position_y_u, sizeof(position_y_u)); + std::memcpy(&position_y_std_f, &position_y_std_u, sizeof(position_y_std_u)); + std::memcpy(&position_z_f, &position_z_u, sizeof(position_z_u)); + std::memcpy(&position_z_std_f, &position_z_std_u, sizeof(position_z_std_u)); + std::memcpy( + &position_xy_covariance_f, &position_xy_covariance_u, sizeof(position_xy_covariance_u)); + std::memcpy(&position_orientation_f, &position_orientation_u, sizeof(position_orientation_u)); + std::memcpy( + &position_orientation_std_f, &position_orientation_std_u, sizeof(position_orientation_std_u)); + std::memcpy( + &existence_probability_f, &existence_probability_u, sizeof(existence_probability_u)); + + const uint8_t classification_car_u = data[CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE]; + const uint8_t classification_truck_u = data[CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE]; + const uint8_t classification_motorcycle_u = data[CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE]; + const uint8_t classification_bicycle_u = data[CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE]; + const uint8_t classification_pedestrian_u = data[CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE]; + const uint8_t classification_animal_u = data[CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE]; + const uint8_t classification_hazard_u = data[CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE]; + const uint8_t classification_unknown_u = data[CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE]; + + const uint32_t dynamics_abs_vel_x_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 3]; + const uint32_t dynamics_abs_vel_x_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 3]; + const uint32_t dynamics_abs_vel_y_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 3]; + const uint32_t dynamics_abs_vel_y_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 3]; + const uint32_t dynamics_abs_vel_covariance_xy_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 3]; + + const uint32_t dynamics_rel_vel_x_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 3]; + const uint32_t dynamics_rel_vel_x_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 3]; + const uint32_t dynamics_rel_vel_y_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 3]; + const uint32_t dynamics_rel_vel_y_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 3]; + const uint32_t dynamics_rel_vel_covariance_xy_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 3]; + + const uint32_t dynamics_abs_accel_x_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 3]; + const uint32_t dynamics_abs_accel_x_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 3]; + const uint32_t dynamics_abs_accel_y_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 3]; + const uint32_t dynamics_abs_accel_y_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 3]; + const uint32_t dynamics_abs_accel_covariance_xy_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 1]) + << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 3]; + + const uint32_t dynamics_rel_accel_x_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 3]; + const uint32_t dynamics_rel_accel_x_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 3]; + const uint32_t dynamics_rel_accel_y_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 3]; + ; + const uint32_t dynamics_rel_accel_y_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 3]; + const uint32_t dynamics_rel_accel_covariance_xy_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 1]) + << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 3]; + + float dynamics_abs_vel_x_f; + float dynamics_abs_vel_x_std_f; + float dynamics_abs_vel_y_f; + float dynamics_abs_vel_y_std_f; + float dynamics_abs_vel_covariance_xy_f; + + float dynamics_rel_vel_x_f; + float dynamics_rel_vel_x_std_f; + float dynamics_rel_vel_y_f; + float dynamics_rel_vel_y_std_f; + float dynamics_rel_vel_covariance_xy_f; + + float dynamics_abs_accel_x_f; + float dynamics_abs_accel_x_std_f; + float dynamics_abs_accel_y_f; + float dynamics_abs_accel_y_std_f; + float dynamics_abs_accel_covariance_xy_f; + + float dynamics_rel_accel_x_f; + float dynamics_rel_accel_x_std_f; + float dynamics_rel_accel_y_f; + float dynamics_rel_accel_y_std_f; + float dynamics_rel_accel_covariance_xy_f; + + std::memcpy(&dynamics_abs_vel_x_f, &dynamics_abs_vel_x_u, sizeof(dynamics_abs_vel_x_u)); + std::memcpy( + &dynamics_abs_vel_x_std_f, &dynamics_abs_vel_x_std_u, sizeof(dynamics_abs_vel_x_std_u)); + std::memcpy(&dynamics_abs_vel_y_f, &dynamics_abs_vel_y_u, sizeof(dynamics_abs_vel_y_u)); + std::memcpy( + &dynamics_abs_vel_y_std_f, &dynamics_abs_vel_y_std_u, sizeof(dynamics_abs_vel_y_std_u)); + std::memcpy( + &dynamics_abs_vel_covariance_xy_f, &dynamics_abs_vel_covariance_xy_u, + sizeof(dynamics_abs_vel_covariance_xy_u)); + + std::memcpy(&dynamics_rel_vel_x_f, &dynamics_rel_vel_x_u, sizeof(dynamics_rel_vel_x_u)); + std::memcpy( + &dynamics_rel_vel_x_std_f, &dynamics_rel_vel_x_std_u, sizeof(dynamics_rel_vel_x_std_u)); + std::memcpy(&dynamics_rel_vel_y_f, &dynamics_rel_vel_y_u, sizeof(dynamics_rel_vel_y_u)); + std::memcpy( + &dynamics_rel_vel_y_std_f, &dynamics_rel_vel_y_std_u, sizeof(dynamics_rel_vel_y_std_u)); + std::memcpy( + &dynamics_rel_vel_covariance_xy_f, &dynamics_rel_vel_covariance_xy_u, + sizeof(dynamics_rel_vel_covariance_xy_u)); + + std::memcpy(&dynamics_abs_accel_x_f, &dynamics_abs_accel_x_u, sizeof(dynamics_abs_accel_x_u)); + std::memcpy( + &dynamics_abs_accel_x_std_f, &dynamics_abs_accel_x_std_u, sizeof(dynamics_abs_accel_x_std_u)); + std::memcpy(&dynamics_abs_accel_y_f, &dynamics_abs_accel_y_u, sizeof(dynamics_abs_accel_y_u)); + std::memcpy( + &dynamics_abs_accel_y_std_f, &dynamics_abs_accel_y_std_u, sizeof(dynamics_abs_accel_y_std_u)); + std::memcpy( + &dynamics_abs_accel_covariance_xy_f, &dynamics_abs_accel_covariance_xy_u, + sizeof(dynamics_abs_accel_covariance_xy_u)); + + std::memcpy(&dynamics_rel_accel_x_f, &dynamics_rel_accel_x_u, sizeof(dynamics_rel_accel_x_u)); + std::memcpy( + &dynamics_rel_accel_x_std_f, &dynamics_rel_accel_x_std_u, sizeof(dynamics_rel_accel_x_std_u)); + std::memcpy(&dynamics_rel_accel_y_f, &dynamics_rel_accel_y_u, sizeof(dynamics_rel_accel_y_u)); + std::memcpy( + &dynamics_rel_accel_y_std_f, &dynamics_rel_accel_y_std_u, sizeof(dynamics_rel_accel_y_std_u)); + std::memcpy( + &dynamics_rel_accel_covariance_xy_f, &dynamics_rel_accel_covariance_xy_u, + sizeof(dynamics_rel_accel_covariance_xy_u)); + + const uint32_t dynamics_orientation_rate_mean_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 3]; + const uint32_t dynamics_orientation_rate_std_u = + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 3]; + + const uint32_t shape_length_edge_mean_u = + (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 3]; + const uint32_t shape_width_edge_mean_u = + (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 3]; + + float dynamics_orientation_rate_mean_f; + float dynamics_orientation_rate_std_f; + + float shape_length_edge_mean_f; + float shape_width_edge_mean_f; + + std::memcpy( + &dynamics_orientation_rate_mean_f, &dynamics_orientation_rate_mean_u, + sizeof(dynamics_orientation_rate_mean_u)); + std::memcpy( + &dynamics_orientation_rate_std_f, &dynamics_orientation_rate_std_u, + sizeof(dynamics_orientation_rate_std_u)); + std::memcpy( + &shape_length_edge_mean_f, &shape_length_edge_mean_u, sizeof(shape_length_edge_mean_u)); + std::memcpy( + &shape_width_edge_mean_f, &shape_width_edge_mean_u, sizeof(shape_width_edge_mean_u)); + + assert(status_measurement_u <= 2 || status_measurement_u == 255); + assert(status_movement_u <= 1 || status_movement_u == 255); + assert(position_reference_u <= 7 || position_reference_u == 255); + + assert(position_x_f >= -1600.f && position_x_f <= 1600.f); + assert(position_x_std_f >= 0.f); + assert(position_y_f >= -1600.f && position_y_f <= 1600.f); + assert(position_y_std_f >= 0.f); + assert(position_z_f >= -1600.f && position_z_f <= 1600.f); + assert(position_z_std_f >= 0.f); + assert(position_orientation_f >= -M_PI && position_orientation_f <= M_PI); + assert(position_orientation_std_f >= 0.f); + + assert(classification_car_u <= 100); + assert(classification_truck_u <= 100); + assert(classification_motorcycle_u <= 100); + assert(classification_bicycle_u <= 100); + assert(classification_pedestrian_u <= 100); + assert(classification_animal_u <= 100); + assert(classification_hazard_u <= 100); + assert(classification_unknown_u <= 100); + + assert(dynamics_abs_vel_x_std_f >= 0.f); + assert(dynamics_abs_vel_y_std_f >= 0.f); + + assert(dynamics_rel_vel_x_std_f >= 0.f); + assert(dynamics_rel_vel_y_std_f >= 0.f); + + assert(dynamics_abs_accel_x_std_f >= 0.f); + assert(dynamics_abs_accel_y_std_f >= 0.f); + + assert(dynamics_rel_accel_x_std_f >= 0.f); + assert(dynamics_rel_accel_y_std_f >= 0.f); + + object_msg.object_id = object_id_u; + object_msg.age = object_age_u; + object_msg.status_measurement = status_measurement_u; + object_msg.status_movement = status_movement_u; + object_msg.position_reference = position_reference_u; + + object_msg.position.x = static_cast(position_x_f); + object_msg.position.y = static_cast(position_y_f); + object_msg.position.y = static_cast(position_z_f); + + object_msg.position_std.x = static_cast(position_x_std_f); + object_msg.position_std.y = static_cast(position_y_std_f); + object_msg.position_std.z = static_cast(position_z_std_f); + + object_msg.position_covariance_xy = position_xy_covariance_f; + + object_msg.position_orientation = position_orientation_f; + object_msg.position_orientation_std = position_orientation_std_f; + + object_msg.existence_probability = existence_probability_f; + object_msg.classification_car = classification_car_u; + object_msg.classification_truck = classification_truck_u; + object_msg.classification_motorcycle = classification_motorcycle_u; + object_msg.classification_bicycle = classification_bicycle_u; + object_msg.classification_pedestrian = classification_pedestrian_u; + object_msg.classification_animal = classification_animal_u; + object_msg.classification_hazard = classification_hazard_u; + object_msg.classification_unknown = classification_unknown_u; + + object_msg.absolute_velocity.x = static_cast(dynamics_abs_vel_x_f); + object_msg.absolute_velocity.y = static_cast(dynamics_abs_vel_y_f); + object_msg.absolute_velocity_std.x = static_cast(dynamics_abs_vel_x_std_f); + object_msg.absolute_velocity_std.y = static_cast(dynamics_abs_vel_y_std_f); + object_msg.absolute_velocity_covariance_xy = dynamics_abs_vel_covariance_xy_f; + + object_msg.relative_velocity.x = static_cast(dynamics_rel_vel_x_f); + object_msg.relative_velocity.y = static_cast(dynamics_rel_vel_y_f); + object_msg.relative_velocity_std.x = static_cast(dynamics_rel_vel_x_std_f); + object_msg.relative_velocity_std.y = static_cast(dynamics_rel_vel_y_std_f); + object_msg.relative_velocity_covariance_xy = dynamics_rel_vel_covariance_xy_f; + + object_msg.absolute_acceleration.x = static_cast(dynamics_abs_accel_x_f); + object_msg.absolute_acceleration.y = static_cast(dynamics_abs_accel_y_f); + object_msg.absolute_acceleration_std.x = static_cast(dynamics_abs_accel_x_std_f); + object_msg.absolute_acceleration_std.y = static_cast(dynamics_abs_accel_y_std_f); + object_msg.absolute_acceleration_covariance_xy = dynamics_abs_accel_covariance_xy_f; + + object_msg.relative_velocity.x = dynamics_rel_accel_x_f; + object_msg.relative_velocity.y = dynamics_rel_accel_y_f; + object_msg.relative_velocity_std.x = dynamics_rel_accel_x_std_f; + object_msg.relative_velocity_std.y = dynamics_rel_accel_y_std_f; + object_msg.relative_velocity_covariance_xy = dynamics_rel_accel_covariance_xy_f; + + object_msg.orientation_rate_mean = dynamics_orientation_rate_mean_f; + object_msg.orientation_rate_std = dynamics_orientation_rate_std_f; + + object_msg.shape_length_edge_mean = shape_length_edge_mean_f; + object_msg.shape_width_edge_mean = shape_width_edge_mean_f; + } + + object_list_callback_(std::move(msg_ptr)); + + return true; +} + +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index b0e15b4d5..367e52362 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -38,6 +38,11 @@ ament_auto_add_library(nebula_hw_interfaces_robosense SHARED src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp ) +ament_auto_add_library(nebula_hw_interfaces_continental SHARED + src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp + ) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 026e55d63..c3b405a74 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -1,10 +1,25 @@ +// Copyright 2023 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_HW_INTERFACE_BASE_H #define NEBULA_HW_INTERFACE_BASE_H +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" -#include "boost_udp_driver/udp_driver.hpp" +#include #include #include #include @@ -59,7 +74,10 @@ class NebulaHwInterfaceBase /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status virtual Status GetCalibrationConfiguration( - CalibrationConfigurationBase & calibration_configuration) = 0; + [[maybe_unused]] CalibrationConfigurationBase & calibration_configuration) + { + return Status::NOT_IMPLEMENTED; + } }; } // namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp new file mode 100644 index 000000000..a9879cdcc --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp @@ -0,0 +1,252 @@ +// Copyright 2023 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_RADAR_ETHERNET_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_RADAR_ETHERNET_HW_INTERFACE_H +// Have to define macros to silence warnings about deprecated headers being used by +// boost/property_tree/ in some versions of boost. +// See: https://github.com/boostorg/property_tree/issues/51 +#include +#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 +#define BOOST_BIND_GLOBAL_PLACEHOLDERS +#endif +#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#endif +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" +#include "nebula_common/continental/continental_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp" + +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +constexpr int SERVICE_ID_BYTE = 0; +constexpr int METHOD_ID_BYTE = 2; +constexpr int LENGTH_BYTE = 4; + +constexpr int CONFIGURATION_METHOD_ID = 390; +constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; + +constexpr int STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; +constexpr int STATUS_TIMESTAMP_SECONDS_BYTE = 12; +constexpr int STATUS_SYNC_STATUS_BYTE = 16; +constexpr int STATUS_SW_VERSION_MAJOR_BYTE = 17; +constexpr int STATUS_SW_VERSION_MINOR_BYTE = 18; +constexpr int STATUS_SW_VERSION_PATCH_BYTE = 19; + +constexpr int STATUS_LONGITUDINAL_BYTE = 20; +constexpr int STATUS_LATERAL_BYTE = 24; +constexpr int STATUS_VERTICAL_BYTE = 28; +constexpr int STATUS_YAW_BYTE = 32; +constexpr int STATUS_PITCH_BYTE = 36; + +constexpr int STATUS_PLUG_ORIENTATION_BYTE = 40; +constexpr int STATUS_LENGTH_BYTE = 41; +constexpr int STATUS_WIDTH_BYTE = 45; +constexpr int STATUS_HEIGHT_BYTE = 49; +constexpr int STATUS_WHEEL_BASE_BYTE = 53; +constexpr int STATUS_MAXIMUM_DISTANCE_BYTE = 57; +constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; +constexpr int STATUS_CYCLE_TIME_BYTE = 60; +constexpr int STATUS_TIME_SLOT_BYTE = 61; +constexpr int STATUS_HCC_BYTE = 62; +constexpr int STATUS_POWERSAVING_STANDSTILL_BYTE = 63; +constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; +constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; +constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; +constexpr int STATUS_LONGITUDINAL_VELOCITY_BYTE = 73; +constexpr int STATUS_LONGITUDINAL_ACCELERATION_BYTE = 74; +constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; +constexpr int STATUS_YAW_RATE_BYTE = 76; +constexpr int STATUS_STEERING_ANGLE_BYTE = 77; +constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; +constexpr int STATUS_CHARASTERISTIC_SPEED_BYTE = 79; +constexpr int STATUS_RADAR_STATUS_BYTE = 80; +constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; +constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; +constexpr int STATUS_BLOCKAGE_STATUS_BYTE = 83; + +/// @brief Hardware interface of hesai driver +class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase +{ +private: + std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; + std::shared_ptr sensor_configuration_; + std::unique_ptr nebula_packets_ptr_; + std::function buffer)> + nebula_packets_reception_callback_; + + std::mutex sensor_status_mutex_; + + // KL move these to the continental header file + static constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; + static constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; + + struct FilterStatus + { + uint8_t active; + uint8_t filter_id; + uint8_t min_value; + uint8_t max_value; + }; + + FilterStatus detection_filters_status_[DETECTION_FILTER_PROPERTIES_NUM]; + FilterStatus object_filters_status_[OBJECT_FILTER_PROPERTIES_NUM]; + + ContinentalRadarStatus radar_status_; + + std::shared_ptr parent_node_logger; + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM + /// @param bytes Target byte vector + void PrintDebug(const std::vector & bytes); + +public: + /// @brief Constructor + ContinentalRadarEthernetHwInterface(); + /// @brief Initializing tcp_driver for TCP communication + /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration + /// @return Resulting status + + void ProcessSensorStatusPacket(const std::vector & buffer); + void ProcessFilterStatusPacket(const std::vector & buffer); + void ProcessDataPacket(const std::vector & buffer); + + /// @brief Parsing json string to property_tree + /// @param str JSON string + /// @return Parsed property_tree + boost::property_tree::ptree ParseJson(const std::string & str); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveCloudPacketCallbackWithSender( + const std::vector & buffer, const std::string & sender_ip); + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveCloudPacketCallback(const std::vector & buffer) final; + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status CloudInterfaceStart() final; + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status CloudInterfaceStop() final; + /// @brief Printing sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + std::shared_ptr sensor_configuration) final; + /// @brief Registering callback for PandarScan + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterScanCallback( + std::function)> scan_callback); + + /// @brief @brief Setting SensorMounting + /// @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 pitch_autosar Desired pitch value in autosar coordinates + /// @param plug_orientation Desired plug orientation (0 = PLUG_RIGHT, 1 = PLUG_LEFT) + /// @return Resulting status + Status SetSensorMounting( + float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, + float pitch_autosar, uint8_t plug_orientation); + + /// @brief @brief Setting SensorMounting + /// @param length_autosar Desired vehicle length value + /// @param width_autosar Desired vehicle width value + /// @param height_autosar Desired height value + /// @param wheel_base_autosar Desired wheel base value + /// @return Resulting status + Status SetVehicleParameters( + float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar); + + /// @brief @brief Setting RadarParameters + /// @param maximum_distance Desired maximum detection distance (93m <= v <= 1514m) + /// @param frequency_slot Desired frequency slot (0 = Low (76.23 GHz), 1 = Mid (76.48 GHz), 2 = + /// High (76.73 GHz)) + /// @param cycle_time Desired cycle time value (50ms <= v <= 100ms) + /// @param time_slot Desired time slot value (10ms <= v <= 90ms) + /// @param hcc Desired hcc value (1 = Worldwide, 2 = Japan) + /// @param powersave_standstill Desired powersave_standstill value (0 = Off, 1 = On) + /// @return Resulting status + Status SetRadarParameters( + uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, + uint8_t hcc, uint8_t powersave_standstill); + + /// @brief @brief Setting RadarParameters + /// @param sensor_ip_address Desired sensor ip address + /// @param with_run Automatically executes run() of TcpDriver + /// @return Resulting status + Status SetSensorIPAddress(const std::string & sensor_ip_address); + + Status SetAccelerationLateralCog(float lateral_acceleration); + Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); + Status SetCharasteristicSpeed(float charasteristic_speed); + Status SetDrivingDirection(int direction); + Status SetSteeringAngleFrontAxle(float angle_rad); + Status SetVelocityVehicle(float velocity); + Status SetYawRate(float yaw_rate); + + Status GetLidarMonitor(std::shared_ptr ctx, bool with_run = true); + Status GetLidarMonitor(bool with_run = true); + + /// @brief Checking the current settings and changing the difference point + /// @return Resulting status + Status CheckAndSetConfig(); + + /// @brief Returns the last semantic sensor status + /// @return Last semantic sensor status message + ContinentalRadarStatus GetRadarStatus(); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); +}; +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_CONTINENTAL_RADAR_ETHERNET_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp new file mode 100644 index 000000000..b3f3646b0 --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 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 CONTINENTAL_TYPES_HPP +#define CONTINENTAL_TYPES_HPP + +#include +#include + +#include +#include + +namespace nebula +{ + +/// @brief semantic struct of Radar Status (overfitted to ARS548) +struct ContinentalRadarStatus +{ + uint32_t timestamp_nanoseconds; + uint32_t timestamp_seconds; + std::string timestamp_sync_status; + uint8_t sw_version_major; + uint8_t sw_version_minor; + uint8_t sw_version_patch; + float longitudinal; + float lateral; + float vertical; + float yaw; + float pitch; + std::string plug_orientation; + float length; + float width; + float height; + float wheel_base; + uint16_t max_distance; + std::string frequency_slot; + uint8_t cycle_time; + uint8_t time_slot; + std::string hcc; + std::string powersave_standstill; + std::string sensor_ip_address0; + std::string sensor_ip_address1; + uint8_t configuration_counter; + std::string status_longitudinal_velocity; + std::string status_longitudinal_acceleration; + std::string status_lateral_acceleration; + std::string status_yaw_rate; + std::string status_steering_angle; + std::string status_driving_direction; + std::string charasteristic_speed; + std::string radar_status; + std::string voltage_status; + std::string temperature_status; + std::string blockage_status; + + ContinentalRadarStatus() {} + + friend std::ostream & operator<<(std::ostream & os, nebula::ContinentalRadarStatus const & arg) + { + os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; + os << ", "; + os << "timestamp_seconds: " << arg.timestamp_seconds; + os << ", "; + os << "timestamp_sync_status: " << arg.timestamp_sync_status; + os << ", "; + os << "sw_version_major: " << arg.sw_version_major; + os << ", "; + os << "sw_version_minor: " << arg.sw_version_minor; + os << ", "; + os << "sw_version_patch: " << arg.sw_version_patch; + os << ", "; + os << "longitudinal: " << arg.longitudinal; + os << ", "; + os << "lateral: " << arg.lateral; + os << ", "; + os << "vertical: " << arg.vertical; + os << ", "; + os << "yaw: " << arg.yaw; + os << ", "; + os << "pitch: " << arg.pitch; + os << ", "; + os << "plug_orientation: " << arg.plug_orientation; + os << ", "; + os << "length: " << arg.length; + os << ", "; + os << "width: " << arg.width; + os << ", "; + os << "height: " << arg.height; + os << ", "; + os << "wheel_base: " << arg.wheel_base; + os << ", "; + os << "max_distance: " << arg.max_distance; + os << ", "; + os << "frequency_slot: " << arg.frequency_slot; + os << ", "; + os << "cycle_time: " << arg.cycle_time; + os << ", "; + os << "time_slot: " << arg.time_slot; + os << ", "; + os << "hcc: " << arg.hcc; + os << ", "; + os << "powersave_standstill: " << arg.powersave_standstill; + os << ", "; + os << "sensor_ip_address0: " << arg.sensor_ip_address0; + os << ", "; + os << "sensor_ip_address1: " << arg.sensor_ip_address1; + os << ", "; + os << "configuration_counter: " << arg.configuration_counter; + os << ", "; + os << "status_longitudinal_velocity: " << arg.status_longitudinal_velocity; + os << ", "; + os << "status_longitudinal_acceleration: " << arg.status_longitudinal_acceleration; + os << ", "; + os << "status_lateral_acceleration: " << arg.status_lateral_acceleration; + os << ", "; + os << "status_yaw_rate: " << arg.status_yaw_rate; + os << ", "; + os << "status_steering_angle: " << arg.status_steering_angle; + os << ", "; + os << "status_drivin_direction: " << arg.status_driving_direction; + os << ", "; + os << "charasteristic_speed: " << arg.charasteristic_speed; + os << ", "; + os << "radar_status: " << arg.radar_status; + os << ", "; + os << "temperature_status: " << arg.temperature_status; + os << ", "; + os << "voltage_status: " << arg.voltage_status; + os << ", "; + os << "blockage_status_rate: " << arg.blockage_status; + + return os; + } +}; + +} // namespace nebula +#endif // HESAI_CMD_RESPONSE_HPP diff --git a/nebula_hw_interfaces/package.xml b/nebula_hw_interfaces/package.xml index 25bdf7f8e..b526108c9 100644 --- a/nebula_hw_interfaces/package.xml +++ b/nebula_hw_interfaces/package.xml @@ -11,15 +11,16 @@ ament_cmake_auto + boost_tcp_driver + boost_udp_driver libpcl-all-dev nebula_common + nebula_msgs pandar_msgs rclcpp + robosense_msgs sensor_msgs - boost_tcp_driver - boost_udp_driver velodyne_msgs - robosense_msgs ament_cmake_gtest ament_lint_auto diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp new file mode 100644 index 000000000..878ad25c0 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp @@ -0,0 +1,995 @@ +// Copyright 2023 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_radar_ethernet_hw_interface.hpp" + +#include + +namespace nebula +{ +namespace drivers +{ +ContinentalRadarEthernetHwInterface::ContinentalRadarEthernetHwInterface() +: cloud_io_context_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, + nebula_packets_ptr_{std::make_unique()} +{ +} + +Status ContinentalRadarEthernetHwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + Status status = Status::OK; + + try { + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); + } catch (const std::exception & ex) { + status = Status::SENSOR_CONFIG_ERROR; + std::cerr << status << std::endl; + return status; + } + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::CloudInterfaceStart() +{ + try { + std::cout << "Starting Data UDP server on: " << *sensor_configuration_ << std::endl; + sensor_udp_driver_->init_receiver( + sensor_configuration_->multicast_ip, sensor_configuration_->data_port, + sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); + sensor_udp_driver_->receiver()->setMulticast(true); + sensor_udp_driver_->receiver()->open(); + sensor_udp_driver_->receiver()->bind(); + sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( + &ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallbackWithSender, this, + std::placeholders::_1, std::placeholders::_2)); + + sensor_udp_driver_->init_sender( + sensor_configuration_->sensor_ip, sensor_configuration_->configuration_sensor_port, + sensor_configuration_->host_ip, sensor_configuration_->configuration_host_port); + + sensor_udp_driver_->sender()->open(); + sensor_udp_driver_->sender()->bind(); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + } catch (const std::exception & ex) { + Status status = Status::UDP_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->sensor_ip << "," + << sensor_configuration_->data_port << std::endl; + return status; + } + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::RegisterScanCallback( + std::function)> callback) +{ + nebula_packets_reception_callback_ = std::move(callback); + return Status::OK; +} + +void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallbackWithSender( + const std::vector & buffer, const std::string & sender_ip) +{ + if (sender_ip == sensor_configuration_->sensor_ip) { + ReceiveCloudPacketCallback(buffer); + } +} +void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallback( + const std::vector & buffer) +{ + constexpr int DETECTION_LIST_METHOD_ID = 336; + constexpr int OBJECT_LIST_METHOD_ID = 329; + constexpr int SENSOR_STATUS_METHOD_ID = 380; + constexpr int FILTER_STATUS_METHOD_ID = 396; + + constexpr int DETECTION_LIST_UDP_PAYPLOAD = 35336; + constexpr int OBJECT_LIST_UDP_PAYPLOAD = 9401; + constexpr int SENSOR_STATUS_UDP_PAYPLOAD = 84; + constexpr int FILTER_STATUS_UDP_PAYPLOAD = 330; + + constexpr int DETECTION_LIST_PDU_LENGTH = 35328; + constexpr int OBJECT_LIST_PDU_LENGTH = 9393; + constexpr int SENSOR_STATUS_PDU_LENGTH = 76; + constexpr int FILTER_STATUS_PDU_LENGTH = 322; + + if (buffer.size() < LENGTH_BYTE + sizeof(uint32_t)) { + PrintError("Unrecognized packet. Too short"); + return; + } + + const int service_id = + (static_cast(buffer[SERVICE_ID_BYTE]) << 8) | buffer[SERVICE_ID_BYTE + 1]; + const int method_id = + (static_cast(buffer[METHOD_ID_BYTE]) << 8) | buffer[METHOD_ID_BYTE + 1]; + const int length = (static_cast(buffer[LENGTH_BYTE]) << 24) | + (static_cast(buffer[LENGTH_BYTE + 1]) << 16) | + (static_cast(buffer[LENGTH_BYTE + 2]) << 8) | + buffer[LENGTH_BYTE + 3]; + + if (service_id != 0) { + PrintError("Invalid service id"); + return; + } else if (method_id == SENSOR_STATUS_METHOD_ID) { + if (buffer.size() != SENSOR_STATUS_UDP_PAYPLOAD || length != SENSOR_STATUS_PDU_LENGTH) { + PrintError("SensorStatus message with invalid size"); + return; + } + ProcessSensorStatusPacket(buffer); + } else if (method_id == FILTER_STATUS_METHOD_ID) { + if (buffer.size() != FILTER_STATUS_UDP_PAYPLOAD || length != FILTER_STATUS_PDU_LENGTH) { + PrintError("FilterStatus message with invalid size"); + return; + } + + ProcessFilterStatusPacket(buffer); + } else if (method_id == DETECTION_LIST_METHOD_ID) { + if (buffer.size() != DETECTION_LIST_UDP_PAYPLOAD || length != DETECTION_LIST_PDU_LENGTH) { + PrintError("DetectionList message with invalid size"); + return; + } + + ProcessDataPacket(buffer); + } else if (method_id == OBJECT_LIST_METHOD_ID) { + if (buffer.size() != OBJECT_LIST_UDP_PAYPLOAD || length != OBJECT_LIST_PDU_LENGTH) { + PrintError("ObjectList message with invalid size"); + return; + } + + ProcessDataPacket(buffer); + } +} + +void ContinentalRadarEthernetHwInterface::ProcessSensorStatusPacket( + const std::vector & buffer) +{ + radar_status_.timestamp_nanoseconds = + (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | + (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | + buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; + radar_status_.timestamp_seconds = + (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | + (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | + buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 3]; + + const uint8_t & sync_status = buffer[STATUS_SYNC_STATUS_BYTE]; + if (sync_status == 1) { + radar_status_.timestamp_sync_status = "SYNC_OK"; + } else if (sync_status == 2) { + radar_status_.timestamp_sync_status = "NEVER_SYNC"; + } else if (sync_status == 3) { + radar_status_.timestamp_sync_status = "SYNC_LOST"; + } else { + radar_status_.timestamp_sync_status = "INVALID_VALUE"; + } + + radar_status_.timestamp_sync_status = buffer[STATUS_SYNC_STATUS_BYTE]; + radar_status_.sw_version_major = buffer[STATUS_SW_VERSION_MAJOR_BYTE]; + radar_status_.sw_version_minor = buffer[STATUS_SW_VERSION_MINOR_BYTE]; + radar_status_.sw_version_patch = buffer[STATUS_SW_VERSION_PATCH_BYTE]; + + const uint32_t status_longitudinal_u = + (static_cast(buffer[STATUS_LONGITUDINAL_BYTE]) << 24) | + (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 2]) << 8) | + buffer[STATUS_LONGITUDINAL_BYTE + 3]; + const uint32_t status_lateral_u = (static_cast(buffer[STATUS_LATERAL_BYTE]) << 24) | + (static_cast(buffer[STATUS_LATERAL_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_LATERAL_BYTE + 2]) << 8) | + buffer[STATUS_LATERAL_BYTE + 3]; + const uint32_t status_vertical_u = + (static_cast(buffer[STATUS_VERTICAL_BYTE]) << 24) | + (static_cast(buffer[STATUS_VERTICAL_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_VERTICAL_BYTE + 2]) << 8) | + buffer[STATUS_VERTICAL_BYTE + 3]; + const uint32_t status_yaw_u = (static_cast(buffer[STATUS_YAW_BYTE]) << 24) | + (static_cast(buffer[STATUS_YAW_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_YAW_BYTE + 2]) << 8) | + buffer[STATUS_YAW_BYTE + 3]; + const uint32_t status_pitch_u = (static_cast(buffer[STATUS_PITCH_BYTE]) << 24) | + (static_cast(buffer[STATUS_PITCH_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_PITCH_BYTE + 2]) << 8) | + buffer[STATUS_PITCH_BYTE + 3]; + const uint8_t & plug_orientation = buffer[STATUS_PLUG_ORIENTATION_BYTE]; + radar_status_.plug_orientation = plug_orientation == 0 ? "PLUG_RIGHT" + : plug_orientation == 1 ? "PLUG_LEFT" + : "INVALID_VALUE"; + const uint32_t status_length_u = (static_cast(buffer[STATUS_LENGTH_BYTE]) << 24) | + (static_cast(buffer[STATUS_LENGTH_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_LENGTH_BYTE + 2]) << 8) | + buffer[STATUS_LENGTH_BYTE + 3]; + const uint32_t status_width_u = (static_cast(buffer[STATUS_WIDTH_BYTE]) << 24) | + (static_cast(buffer[STATUS_WIDTH_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_WIDTH_BYTE + 2]) << 8) | + buffer[STATUS_WIDTH_BYTE + 3]; + const uint32_t status_height_u = (static_cast(buffer[STATUS_HEIGHT_BYTE]) << 24) | + (static_cast(buffer[STATUS_HEIGHT_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_HEIGHT_BYTE + 2]) << 8) | + buffer[STATUS_HEIGHT_BYTE + 3]; + const uint32_t status_wheel_base_u = + (static_cast(buffer[STATUS_WHEEL_BASE_BYTE]) << 24) | + (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 1]) << 16) | + (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 2]) << 8) | + buffer[STATUS_WHEEL_BASE_BYTE + 3]; + radar_status_.max_distance = (static_cast(buffer[STATUS_MAXIMUM_DISTANCE_BYTE]) << 8) | + buffer[STATUS_MAXIMUM_DISTANCE_BYTE + 1]; + + std::memcpy(&radar_status_.longitudinal, &status_longitudinal_u, sizeof(status_longitudinal_u)); + std::memcpy(&radar_status_.lateral, &status_lateral_u, sizeof(status_lateral_u)); + std::memcpy(&radar_status_.vertical, &status_vertical_u, sizeof(status_vertical_u)); + std::memcpy(&radar_status_.yaw, &status_yaw_u, sizeof(status_yaw_u)); + + std::memcpy(&radar_status_.pitch, &status_pitch_u, sizeof(status_pitch_u)); + std::memcpy(&radar_status_.length, &status_length_u, sizeof(status_length_u)); + std::memcpy(&radar_status_.width, &status_width_u, sizeof(status_width_u)); + std::memcpy(&radar_status_.height, &status_height_u, sizeof(status_height_u)); + std::memcpy(&radar_status_.wheel_base, &status_wheel_base_u, sizeof(status_wheel_base_u)); + + const uint8_t & frequency_slot = buffer[STATUS_FREQUENCY_SLOT_BYTE]; + + if (frequency_slot == 0) { + radar_status_.frequency_slot = "0:Low (76.23 GHz)"; + } else if (frequency_slot == 1) { + radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; + } else if (frequency_slot == 2) { + radar_status_.frequency_slot = "2:High (76.73 GHz)"; + } else { + radar_status_.frequency_slot = "INVALID VALUE"; + } + + radar_status_.cycle_time = buffer[STATUS_CYCLE_TIME_BYTE]; + radar_status_.time_slot = buffer[STATUS_TIME_SLOT_BYTE]; + + const uint8_t & hcc = buffer[STATUS_HCC_BYTE]; + radar_status_.hcc = hcc == 1 ? "Worldwide" + : hcc == 2 ? "Japan" + : ("INVALID VALUE=" + std::to_string(hcc)); + + const uint8_t & powersave_standstill = buffer[STATUS_POWERSAVING_STANDSTILL_BYTE]; + radar_status_.powersave_standstill = powersave_standstill == 0 ? "Off" + : powersave_standstill == 1 ? "On" + : "INVALID VALUE"; + + const uint8_t status_sensor_ip_address00 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE]; + const uint8_t status_sensor_ip_address01 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 1]; + const uint8_t status_sensor_ip_address02 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 2]; + const uint8_t status_sensor_ip_address03 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 3]; + const uint8_t status_sensor_ip_address10 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE]; + const uint8_t status_sensor_ip_address11 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 1]; + const uint8_t status_sensor_ip_address12 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 2]; + const uint8_t status_sensor_ip_address13 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 3]; + + std::stringstream ss0, ss1; + ss0 << std::to_string(status_sensor_ip_address00) << "." + << std::to_string(status_sensor_ip_address01) << "." + << std::to_string(status_sensor_ip_address02) << "." + << std::to_string(status_sensor_ip_address03); + radar_status_.sensor_ip_address0 = ss0.str(); + + ss1 << std::to_string(status_sensor_ip_address10) << "." + << std::to_string(status_sensor_ip_address11) << "." + << std::to_string(status_sensor_ip_address12) << "." + << std::to_string(status_sensor_ip_address13); + radar_status_.sensor_ip_address1 = ss1.str(); + + radar_status_.configuration_counter = buffer[STATUS_CONFIGURATION_COUNTER_BYTE]; + + const uint8_t & status_longitudinal_velicity = buffer[STATUS_LONGITUDINAL_VELOCITY_BYTE]; + radar_status_.status_longitudinal_velocity = status_longitudinal_velicity == 0 ? "VDY_OK" + : status_longitudinal_velicity == 1 + ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & status_longitudinal_acceleration = buffer[STATUS_LONGITUDINAL_ACCELERATION_BYTE]; + radar_status_.status_longitudinal_acceleration = status_longitudinal_acceleration == 0 ? "VDY_OK" + : status_longitudinal_acceleration == 1 + ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & status_lateral_acceleration = buffer[STATUS_LATERAL_ACCELERATION_BYTE]; + radar_status_.status_lateral_acceleration = status_lateral_acceleration == 0 ? "VDY_OK" + : status_lateral_acceleration == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & status_yaw_rate = buffer[STATUS_YAW_RATE_BYTE]; + radar_status_.status_yaw_rate = status_yaw_rate == 0 ? "VDY_OK" + : status_yaw_rate == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & status_steering_angle = buffer[STATUS_STEERING_ANGLE_BYTE]; + radar_status_.status_steering_angle = status_steering_angle == 0 ? "VDY_OK" + : status_steering_angle == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & status_driving_direction = buffer[STATUS_DRIVING_DIRECTION_BYTE]; + radar_status_.status_driving_direction = status_driving_direction == 0 ? "VDY_OK" + : status_driving_direction == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & charasteristic_speed = buffer[STATUS_CHARASTERISTIC_SPEED_BYTE]; + radar_status_.charasteristic_speed = charasteristic_speed == 0 ? "VDY_OK" + : charasteristic_speed == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + const uint8_t & radar_status = buffer[STATUS_RADAR_STATUS_BYTE]; + if (radar_status == 0) { + radar_status_.radar_status = "STATE_INIT"; + } else if (radar_status == 1) { + radar_status_.radar_status = "STATE_OK"; + } else if (radar_status == 2) { + radar_status_.radar_status = "STATE_INVALID"; + } else { + radar_status_.radar_status = "INVALID VALUE"; + } + + const uint8_t & voltage_status = buffer[STATUS_VOLTAGE_STATUS_BYTE]; + if (voltage_status == 0) { + radar_status_.voltage_status = "Ok"; + } + if (voltage_status & 0x01) { + radar_status_.voltage_status += "Current undervoltage"; + } + if (voltage_status & 0x02) { + radar_status_.voltage_status = "Past undervoltage"; + } + if (voltage_status & 0x03) { + radar_status_.voltage_status = "Current overvoltage"; + } + if (voltage_status & 0x04) { + radar_status_.voltage_status = "Past overvoltage"; + } + + const uint8_t & temperature_status = buffer[STATUS_TEMPERATURE_STATUS_BYTE]; + if (temperature_status == 0) { + radar_status_.temperature_status = "Ok"; + } + if (temperature_status & 0x01) { + radar_status_.temperature_status += "Current undertemperature"; + } + if (temperature_status & 0x02) { + radar_status_.temperature_status += "Past undertemperature"; + } + if (temperature_status & 0x03) { + radar_status_.temperature_status += "Current overtemperature"; + } + if (temperature_status & 0x04) { + radar_status_.temperature_status += "Past overtemperature"; + } + + const uint8_t & blockage_status = buffer[STATUS_BLOCKAGE_STATUS_BYTE]; + const uint8_t & blockage_status0 = blockage_status & 0x0f; + const uint8_t & blockage_status1 = (blockage_status & 0xf0) >> 4; + + if (blockage_status0 == 0) { + radar_status_.blockage_status = "Blind"; + } else if (blockage_status0 == 1) { + radar_status_.blockage_status = "High"; + } else if (blockage_status0 == 2) { + radar_status_.blockage_status = "Mid"; + } else if (blockage_status0 == 3) { + radar_status_.blockage_status = "Low"; + } else if (blockage_status0 == 4) { + radar_status_.blockage_status = "None"; + } else { + radar_status_.blockage_status = "INVALID VALUE"; + } + + if (blockage_status1 == 0) { + radar_status_.blockage_status += ". Self testfailed"; + } else if (blockage_status1 == 1) { + radar_status_.blockage_status += ". Self testpassed"; + } else if (blockage_status1 == 2) { + radar_status_.blockage_status += ". Selftest ongoing"; + } else { + radar_status_.blockage_status += ". Invalid selftest"; + } +} + +void ContinentalRadarEthernetHwInterface::ProcessFilterStatusPacket( + const std::vector & buffer) +{ + // Unused available data + // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; + // constexpr int FILTER_STATUS_TIMESTAMP_SECONDS_BYTE = 12; + // constexpr int FILTER_STATUS_SYNC_STATUS_BYTE = 16; + // constexpr int FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE = 17; + // constexpr int FILTER_STATUS_DETECTION_SORT_INDEX_BYTE = 18; + // constexpr int FILTER_STATUS_OBJECT_SORT_INDEX_BYTE = 19; + constexpr int FILTER_STATUS_DETECTION_FILTER_BYTE = 20; + constexpr int FILTER_STATUS_OBJECT_FILTER_BYTE = 90; + + // Unused available data + // const uint32_t filter_status_timestamp_nanoseconds = + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | + // buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; const uint32_t + // filter_status_timestamp_seconds = + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | + // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | + // buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 3]; const uint8_t filter_status_sync_status = + // buffer[FILTER_STATUS_SYNC_STATUS_BYTE]; const uint8_t + // filter_status_filter_configuration_counter = + // buffer[FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE]; const uint8_t + // filter_status_detection_sort_index = buffer[FILTER_STATUS_DETECTION_SORT_INDEX_BYTE]; const + // uint8_t filter_status_object_sort_index = buffer[FILTER_STATUS_OBJECT_SORT_INDEX_BYTE]; + + std::size_t byte_index = FILTER_STATUS_DETECTION_FILTER_BYTE; + for (int property_index = 1; property_index <= 7; property_index++) { + FilterStatus filter_status; + filter_status.active = buffer[byte_index++]; + assert(buffer[byte_index] == property_index); + filter_status.filter_id = buffer[byte_index++]; + filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | + (static_cast(buffer[byte_index + 1]) << 16) | + (static_cast(buffer[byte_index + 2]) << 8) | + buffer[byte_index + 3]; + byte_index += 4; + filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | + (static_cast(buffer[byte_index + 1]) << 16) | + (static_cast(buffer[byte_index + 2]) << 8) | + buffer[byte_index + 3]; + byte_index += 4; + + detection_filters_status_[property_index - 1] = filter_status; + } + + byte_index = FILTER_STATUS_OBJECT_FILTER_BYTE; + for (int property_index = 1; property_index <= 24; property_index++) { + FilterStatus filter_status; + filter_status.active = buffer[byte_index++]; + assert(buffer[byte_index] == property_index); + filter_status.filter_id = buffer[byte_index++]; + filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | + (static_cast(buffer[byte_index + 1]) << 16) | + (static_cast(buffer[byte_index + 2]) << 8) | + buffer[byte_index + 3]; + byte_index += 4; + filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | + (static_cast(buffer[byte_index + 1]) << 16) | + (static_cast(buffer[byte_index + 2]) << 8) | + buffer[byte_index + 3]; + byte_index += 4; + + object_filters_status_[property_index - 1] = filter_status; + } +} + +void ContinentalRadarEthernetHwInterface::ProcessDataPacket(const std::vector & buffer) +{ + nebula_msgs::msg::NebulaPacket nebula_packet; + nebula_packet.data = buffer; + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + nebula_packet.stamp.sec = static_cast(now_secs); + nebula_packet.stamp.nanosec = + static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); + nebula_packets_ptr_->packets.emplace_back(nebula_packet); + + nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; + nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + nebula_packets_reception_callback_(std::move(nebula_packets_ptr_)); + nebula_packets_ptr_ = std::make_unique(); +} + +Status ContinentalRadarEthernetHwInterface::CloudInterfaceStop() +{ + return Status::ERROR_1; +} + +Status ContinentalRadarEthernetHwInterface::GetSensorConfiguration( + SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + PrintDebug(ss.str()); + return Status::ERROR_1; +} + +boost::property_tree::ptree ContinentalRadarEthernetHwInterface::ParseJson(const std::string & str) +{ + boost::property_tree::ptree tree; + try { + boost::property_tree::read_json(str, tree); + } catch (boost::property_tree::json_parser_error & e) { + std::cerr << e.what() << std::endl; + } + return tree; +} + +Status ContinentalRadarEthernetHwInterface::SetSensorMounting( + float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, + float pitch_autosar, uint8_t plug_orientation) +{ + constexpr int CONFIGURATION_LONGITUDINAL_BYTE = 8; + constexpr int CONFIGURATION_LATERAL_BYTE = 12; + constexpr int CONFIGURATION_VERTICAL_BYTE = 16; + constexpr int CONFIGURATION_YAW_BYTE = 20; + constexpr int CONFIGURATION_PITCH_BYTE = 24; + constexpr int CONFIGURATION_PLUG_ORIENTATION_BYTE = 28; + constexpr int CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE = 60; + + if ( + longitudinal_autosar < -100.f || longitudinal_autosar > 100.f || lateral_autosar < -100.f || + lateral_autosar > 100.f || vertical_autosar < 0.01f || vertical_autosar > 10.f || + yaw_autosar < -M_PI || yaw_autosar > M_PI || pitch_autosar < -M_PI_2 || + pitch_autosar > M_PI_2) { + PrintError("Invalid SetSensorMounting values"); + return Status::SENSOR_CONFIG_ERROR; + } + + std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); + uint8_t longitudinal_autosar_bytes[4]; + uint8_t lateral_autosar_bytes[4]; + uint8_t vertical_autosar_bytes[4]; + uint8_t yaw_autosar_bytes[4]; + uint8_t pitch_autosar_bytes[4]; + std::memcpy(longitudinal_autosar_bytes, &longitudinal_autosar, sizeof(longitudinal_autosar)); + std::memcpy(lateral_autosar_bytes, &lateral_autosar, sizeof(lateral_autosar)); + std::memcpy(vertical_autosar_bytes, &vertical_autosar, sizeof(vertical_autosar)); + std::memcpy(yaw_autosar_bytes, &yaw_autosar, sizeof(yaw_autosar)); + std::memcpy(pitch_autosar_bytes, &pitch_autosar, sizeof(pitch_autosar)); + + send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); + send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); + send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); + send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); + send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); + send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); + + send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 0] = longitudinal_autosar_bytes[3]; + send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 1] = longitudinal_autosar_bytes[2]; + send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 2] = longitudinal_autosar_bytes[1]; + send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 3] = longitudinal_autosar_bytes[0]; + + send_vector[CONFIGURATION_LATERAL_BYTE + 0] = lateral_autosar_bytes[3]; + send_vector[CONFIGURATION_LATERAL_BYTE + 1] = lateral_autosar_bytes[2]; + send_vector[CONFIGURATION_LATERAL_BYTE + 2] = lateral_autosar_bytes[1]; + send_vector[CONFIGURATION_LATERAL_BYTE + 3] = lateral_autosar_bytes[0]; + + send_vector[CONFIGURATION_VERTICAL_BYTE + 0] = vertical_autosar_bytes[3]; + send_vector[CONFIGURATION_VERTICAL_BYTE + 1] = vertical_autosar_bytes[2]; + send_vector[CONFIGURATION_VERTICAL_BYTE + 2] = vertical_autosar_bytes[1]; + send_vector[CONFIGURATION_VERTICAL_BYTE + 3] = vertical_autosar_bytes[0]; + + send_vector[CONFIGURATION_YAW_BYTE + 0] = yaw_autosar_bytes[3]; + send_vector[CONFIGURATION_YAW_BYTE + 1] = yaw_autosar_bytes[2]; + send_vector[CONFIGURATION_YAW_BYTE + 2] = yaw_autosar_bytes[1]; + send_vector[CONFIGURATION_YAW_BYTE + 3] = yaw_autosar_bytes[0]; + + send_vector[CONFIGURATION_PITCH_BYTE + 0] = pitch_autosar_bytes[3]; + send_vector[CONFIGURATION_PITCH_BYTE + 1] = pitch_autosar_bytes[2]; + send_vector[CONFIGURATION_PITCH_BYTE + 2] = pitch_autosar_bytes[1]; + send_vector[CONFIGURATION_PITCH_BYTE + 3] = pitch_autosar_bytes[0]; + + send_vector[CONFIGURATION_PLUG_ORIENTATION_BYTE] = plug_orientation; + send_vector[CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE] = 1; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetVehicleParameters( + float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) +{ + constexpr int CONFIGURATION_LENGTH_BYTE = 29; + constexpr int CONFIGURATION_WIDTH_BYTE = 33; + constexpr int CONFIGURATION_HEIGHT_BYTE = 37; + constexpr int CONFIGURATION_WHEEL_BASE_BYTE = 41; + constexpr int CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE = 61; + + if ( + length_autosar < 0.01f || length_autosar > 100.f || width_autosar < 0.01f || + width_autosar > 100.f || height_autosar < 0.01f || height_autosar > 100.f || + wheel_base_autosar < 0.01f || wheel_base_autosar > 100.f) { + PrintError("Invalid SetVehicleParameters values"); + return Status::SENSOR_CONFIG_ERROR; + } + + std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); + uint8_t length_autosar_bytes[4]; + uint8_t width_autosar_bytes[4]; + uint8_t height_autosar_bytes[4]; + uint8_t wheel_base_autosar_bytes[4]; + std::memcpy(length_autosar_bytes, &length_autosar, sizeof(length_autosar)); + std::memcpy(width_autosar_bytes, &width_autosar, sizeof(width_autosar)); + std::memcpy(height_autosar_bytes, &height_autosar, sizeof(height_autosar)); + std::memcpy(wheel_base_autosar_bytes, &wheel_base_autosar, sizeof(wheel_base_autosar)); + + send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); + send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); + send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); + send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); + send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); + send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); + + send_vector[CONFIGURATION_LENGTH_BYTE + 0] = length_autosar_bytes[3]; + send_vector[CONFIGURATION_LENGTH_BYTE + 1] = length_autosar_bytes[2]; + send_vector[CONFIGURATION_LENGTH_BYTE + 2] = length_autosar_bytes[1]; + send_vector[CONFIGURATION_LENGTH_BYTE + 3] = length_autosar_bytes[0]; + + send_vector[CONFIGURATION_WIDTH_BYTE + 0] = width_autosar_bytes[3]; + send_vector[CONFIGURATION_WIDTH_BYTE + 1] = width_autosar_bytes[2]; + send_vector[CONFIGURATION_WIDTH_BYTE + 2] = width_autosar_bytes[1]; + send_vector[CONFIGURATION_WIDTH_BYTE + 3] = width_autosar_bytes[0]; + + send_vector[CONFIGURATION_HEIGHT_BYTE + 0] = height_autosar_bytes[3]; + send_vector[CONFIGURATION_HEIGHT_BYTE + 1] = height_autosar_bytes[2]; + send_vector[CONFIGURATION_HEIGHT_BYTE + 2] = height_autosar_bytes[1]; + send_vector[CONFIGURATION_HEIGHT_BYTE + 3] = height_autosar_bytes[0]; + + send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 0] = wheel_base_autosar_bytes[3]; + send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 1] = wheel_base_autosar_bytes[2]; + send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 2] = wheel_base_autosar_bytes[1]; + send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 3] = wheel_base_autosar_bytes[0]; + + send_vector[CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE] = 1; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetRadarParameters( + uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, + uint8_t hcc, uint8_t powersave_standstill) +{ + constexpr int CONFIGURATION_MAXIMUM_DISTANCE_BYTE = 45; + constexpr int CONFIGURATION_FREQUENCY_SLOT_BYTE = 47; + constexpr int CONFIGURATION_CYCLE_TIME_BYTE = 48; + constexpr int CONFIGURATION_TIME_SLOT_BYTE = 49; + constexpr int CONFIGURATION_HCC_BYTE = 50; + constexpr int CONFIGURATION_POWERSAVE_STANDSTILL_BYTE = 51; + constexpr int CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE = 62; + + if ( + maximum_distance < 93 || maximum_distance > 1514 || frequency_slot > 2 || cycle_time < 50 || + cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 || + powersave_standstill > 1) { + PrintError("Invalid SetRadarParameters values"); + return Status::SENSOR_CONFIG_ERROR; + } + + std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); + send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); + send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); + send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); + send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); + send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); + send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); + + send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 0] = + static_cast((maximum_distance >> 8) & 0xff); + send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 1] = + static_cast(maximum_distance & 0xff); + + send_vector[CONFIGURATION_FREQUENCY_SLOT_BYTE] = frequency_slot; + send_vector[CONFIGURATION_CYCLE_TIME_BYTE] = cycle_time; + send_vector[CONFIGURATION_TIME_SLOT_BYTE] = time_slot; + send_vector[CONFIGURATION_HCC_BYTE] = hcc; + send_vector[CONFIGURATION_POWERSAVE_STANDSTILL_BYTE] = powersave_standstill; + + send_vector[CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE] = 1; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetSensorIPAddress( + const std::string & sensor_ip_address) +{ + constexpr int CONFIGURATION_SENSOR_IP_ADDRESS0 = 52; + constexpr int CONFIGURATION_SENSOR_IP_ADDRESS1 = 56; + constexpr int CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE = 63; + + std::array ip_bytes; + + try { + auto sensor_ip = boost::asio::ip::address::from_string(sensor_ip_address); + ip_bytes = sensor_ip.to_v4().to_bytes(); + } catch (const std::exception & ex) { + PrintError("Setting invalid IP"); + return Status::SENSOR_CONFIG_ERROR; + } + + std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); + + send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); + send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); + send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); + send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); + send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); + send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); + + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 0] = ip_bytes[0]; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 1] = ip_bytes[1]; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 2] = ip_bytes[2]; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 3] = ip_bytes[3]; + + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 0] = 169; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 1] = 254; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 2] = 116; + send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 3] = 113; + + send_vector[CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE] = 1; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetAccelerationLateralCog(float lateral_acceleration) +{ + constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; + constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; + constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; + const int ACCELERATION_LATERAL_COG_PAYLOAD_SIZE = ACCELERATION_LATERAL_COG_LENGTH + 8; + + if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { + PrintError("Invalid lateral_acceleration value"); + return Status::ERROR_1; + } + + uint8_t bytes[4]; + std::memcpy(bytes, &lateral_acceleration, sizeof(lateral_acceleration)); + + std::vector send_vector(ACCELERATION_LATERAL_COG_PAYLOAD_SIZE, 0); + ; + send_vector[1] = ACCELERATION_LATERAL_COG_SERVICE_ID; + send_vector[2] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID >> 8); + send_vector[3] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID & 0x00ff); + send_vector[7] = ACCELERATION_LATERAL_COG_LENGTH; + send_vector[14] = bytes[3]; + send_vector[15] = bytes[2]; + send_vector[16] = bytes[1]; + send_vector[17] = bytes[0]; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetAccelerationLongitudinalCog( + float longitudinal_acceleration) +{ + constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; + constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; + constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; + const int ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE = ACCELERATION_LONGITUDINAL_COG_LENGTH + 8; + + if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { + PrintError("Invalid longitudinal_acceleration value"); + return Status::ERROR_1; + } + + uint8_t bytes[4]; + std::memcpy(bytes, &longitudinal_acceleration, sizeof(longitudinal_acceleration)); + + std::vector send_vector(ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE, 0); + ; + send_vector[1] = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; + send_vector[2] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID >> 8); + send_vector[3] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID & 0x00ff); + send_vector[7] = ACCELERATION_LONGITUDINAL_COG_LENGTH; + send_vector[14] = bytes[3]; + send_vector[15] = bytes[2]; + send_vector[16] = bytes[1]; + send_vector[17] = bytes[0]; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetCharasteristicSpeed(float charasteristic_speed) +{ + constexpr uint16_t CHARASTERISTIC_SPEED_SERVICE_ID = 0; + constexpr uint16_t CHARASTERISTIC_SPEED_METHOD_ID = 328; + constexpr uint8_t CHARASTERISTIC_SPEED_LENGTH = 11; + const int CHARASTERISTIC_SPEED_PAYLOAD_SIZE = CHARASTERISTIC_SPEED_LENGTH + 8; + + if (charasteristic_speed < 0.f || charasteristic_speed > 255.f) { + PrintError("Invalid charasteristic_speed value"); + return Status::ERROR_1; + } + + std::vector send_vector(CHARASTERISTIC_SPEED_PAYLOAD_SIZE, 0); + ; + send_vector[1] = CHARASTERISTIC_SPEED_SERVICE_ID; + send_vector[2] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID >> 8); + send_vector[3] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID & 0x00ff); + send_vector[7] = CHARASTERISTIC_SPEED_LENGTH; + send_vector[10] = static_cast(charasteristic_speed); + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetDrivingDirection(int direction) +{ + constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; + constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; + constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; + constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; + constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; + constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; + const int DRIVING_DIRECTION_PAYLOAD_SIZE = DRIVING_DIRECTION_LENGTH + 8; + + std::vector send_vector(DRIVING_DIRECTION_PAYLOAD_SIZE, 0); + ; + send_vector[1] = DRIVING_DIRECTION_SERVICE_ID; + send_vector[2] = static_cast(DRIVING_DIRECTION_METHOD_ID >> 8); + send_vector[3] = static_cast(DRIVING_DIRECTION_METHOD_ID & 0xff); + send_vector[7] = DRIVING_DIRECTION_LENGTH; + + if (direction == 0) { + send_vector[9] = DRIVING_DIRECTION_STANDSTILL; + } else if (direction > 0) { + send_vector[9] = DRIVING_DIRECTION_FORWARD; + } else { + send_vector[9] = DRIVING_DIRECTION_BACKWARDS; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetSteeringAngleFrontAxle(float angle_rad) +{ + constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; + constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; + constexpr uint8_t STEERING_ANGLE_LENGTH = 32; + const int STEERING_ANGLE_PAYLOAD_SIZE = STEERING_ANGLE_LENGTH + 8; + + if (angle_rad < -M_PI_2 || angle_rad > M_PI_2) { + PrintError("Invalid angle_rad value"); + return Status::ERROR_1; + } + + uint8_t bytes[4]; + std::memcpy(bytes, &angle_rad, sizeof(angle_rad)); + + std::vector send_vector(STEERING_ANGLE_PAYLOAD_SIZE, 0); + ; + send_vector[1] = STEERING_ANGLE_SERVICE_ID; + send_vector[2] = static_cast(STEERING_ANGLE_METHOD_ID >> 8); + send_vector[3] = static_cast(STEERING_ANGLE_METHOD_ID & 0x00ff); + send_vector[7] = STEERING_ANGLE_LENGTH; + send_vector[14] = bytes[3]; + send_vector[15] = bytes[2]; + send_vector[16] = bytes[1]; + send_vector[17] = bytes[0]; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetVelocityVehicle(float velocity) +{ + constexpr uint16_t VELOCITY_VECHILE_SERVICE_ID = 0; + constexpr uint16_t VELOCITY_VECHILE_METHOD_ID = 323; + constexpr uint8_t VELOCITY_VECHILE_LENGTH = 28; + const int VELOCITY_VECHILE_PAYLOAD_SIZE = VELOCITY_VECHILE_LENGTH + 8; + + uint8_t bytes[4]; + std::memcpy(bytes, &velocity, sizeof(velocity)); + + std::vector send_vector(VELOCITY_VECHILE_PAYLOAD_SIZE, 0); + ; + send_vector[1] = VELOCITY_VECHILE_SERVICE_ID; + send_vector[2] = static_cast(VELOCITY_VECHILE_METHOD_ID >> 8); + send_vector[3] = static_cast(VELOCITY_VECHILE_METHOD_ID & 0x00ff); + send_vector[7] = VELOCITY_VECHILE_LENGTH; + send_vector[11] = bytes[3]; + send_vector[12] = bytes[2]; + send_vector[13] = bytes[1]; + send_vector[14] = bytes[0]; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterface::SetYawRate(float yaw_rate) +{ + constexpr uint16_t YAW_RATE_SERVICE_ID = 0; + constexpr uint16_t YAW_RATE_METHOD_ID = 326; + constexpr uint8_t YAW_RATE_LENGTH = 32; + const int YAW_RATE_PAYLOAD_SIZE = YAW_RATE_LENGTH + 8; + + uint8_t bytes[4]; + std::memcpy(bytes, &yaw_rate, sizeof(yaw_rate)); + + std::vector send_vector(YAW_RATE_PAYLOAD_SIZE, 0); + ; + send_vector[1] = YAW_RATE_SERVICE_ID; + send_vector[2] = static_cast(YAW_RATE_METHOD_ID >> 8); + send_vector[3] = static_cast(YAW_RATE_METHOD_ID & 0x00ff); + send_vector[7] = YAW_RATE_LENGTH; + send_vector[14] = bytes[3]; + send_vector[15] = bytes[2]; + send_vector[16] = bytes[1]; + send_vector[17] = bytes[0]; + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +ContinentalRadarStatus ContinentalRadarEthernetHwInterface::GetRadarStatus() +{ + std::lock_guard l(sensor_status_mutex_); + return radar_status_; +} + +void ContinentalRadarEthernetHwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger = logger; +} + +void ContinentalRadarEthernetHwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger) { + RCLCPP_INFO_STREAM((*parent_node_logger), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalRadarEthernetHwInterface::PrintError(std::string error) +{ + if (parent_node_logger) { + RCLCPP_ERROR_STREAM((*parent_node_logger), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalRadarEthernetHwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger) { + RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + } else { + std::cout << debug << std::endl; + } +} + +void ContinentalRadarEthernetHwInterface::PrintDebug(const std::vector & bytes) +{ + std::stringstream ss; + for (const auto & b : bytes) { + ss << static_cast(b) << ", "; + } + ss << std::endl; + PrintDebug(ss.str()); +} + +} // namespace drivers +} // namespace nebula diff --git a/nebula_messages/continental_msgs/CMakeLists.txt b/nebula_messages/continental_msgs/CMakeLists.txt new file mode 100644 index 000000000..c7b7978f6 --- /dev/null +++ b/nebula_messages/continental_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.5) +project(continental_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ContinentalArs548Detection.msg" + "msg/ContinentalArs548DetectionList.msg" + "msg/ContinentalArs548Object.msg" + "msg/ContinentalArs548ObjectList.msg" + DEPENDENCIES + std_msgs + geometry_msgs + ) + +ament_package() diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg new file mode 100644 index 000000000..a2505ae11 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg @@ -0,0 +1,23 @@ +float32 azimuth_angle +float32 azimuth_angle_std +bool invalid_distance +bool invalid_distance_std +bool invalid_azimuth +bool invalid_azimuth_std +bool invalid_elevation +bool invalid_elevation_std +bool invalid_range_rate +bool invalid_range_rate_std +float32 elevation_angle +float32 elevation_angle_std +float32 range +float32 range_std +float32 range_rate +float32 range_rate_std +int8 rcs +uint16 measurement_id +uint8 positive_predictive_value +uint8 classification +uint8 multi_target_probabilty +uint16 object_id +uint8 ambiguity_flag diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548DetectionList.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548DetectionList.msg new file mode 100644 index 000000000..497400c7d --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548DetectionList.msg @@ -0,0 +1,13 @@ +std_msgs/Header header +uint8 stamp_sync_status +geometry_msgs/Point origin_pos +float32 origin_pitch +float32 origin_pitch_std +float32 origin_yaw +float32 origin_yaw_std +float32 ambiguity_free_velocity_min +float32 ambiguity_free_velocity_max +float32 alignment_azimuth_correction +float32 alignment_elevation_correction +uint8 alignment_status +ContinentalArs548Detection[] detections diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg new file mode 100644 index 000000000..80fca0ad9 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg @@ -0,0 +1,44 @@ +uint32 object_id +uint16 age +uint8 status_measurement +uint8 status_movement +uint8 position_reference + +geometry_msgs/Point position +geometry_msgs/Vector3 position_std +float32 position_covariance_xy +float32 position_orientation +float32 position_orientation_std + +float32 existence_probability + +uint8 classification_car +uint8 classification_truck +uint8 classification_motorcycle +uint8 classification_bicycle +uint8 classification_pedestrian +uint8 classification_animal +uint8 classification_hazard +uint8 classification_unknown + +geometry_msgs/Vector3 absolute_velocity +geometry_msgs/Vector3 absolute_velocity_std +float32 absolute_velocity_covariance_xy + +geometry_msgs/Vector3 relative_velocity +geometry_msgs/Vector3 relative_velocity_std +float32 relative_velocity_covariance_xy + +geometry_msgs/Vector3 absolute_acceleration +geometry_msgs/Vector3 absolute_acceleration_std +float32 absolute_acceleration_covariance_xy + +geometry_msgs/Vector3 relative_acceleration +geometry_msgs/Vector3 relative_acceleration_std +float32 relative_acceleration_covariance_xy + +float32 orientation_rate_mean +float32 orientation_rate_std + +float32 shape_length_edge_mean +float32 shape_width_edge_mean diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548ObjectList.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548ObjectList.msg new file mode 100644 index 000000000..2f8b713f6 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548ObjectList.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +uint8 stamp_sync_status +ContinentalArs548Object[] objects diff --git a/nebula_messages/continental_msgs/msg/NebulaPackets.msg b/nebula_messages/continental_msgs/msg/NebulaPackets.msg new file mode 100644 index 000000000..7aa8444f1 --- /dev/null +++ b/nebula_messages/continental_msgs/msg/NebulaPackets.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +NebulaPacket[] packets diff --git a/nebula_messages/continental_msgs/package.xml b/nebula_messages/continental_msgs/package.xml new file mode 100644 index 000000000..90bb27972 --- /dev/null +++ b/nebula_messages/continental_msgs/package.xml @@ -0,0 +1,26 @@ + + + + continental_msgs + 0.1.0 + Messages for Continental sensors + Kenzo Lobos-Tsunekawa + + Apache 2 + Tier IV + + ament_cmake + rosidl_default_generators + + builtin_interfaces + geometry_msgs + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_messages/nebula_msgs/CMakeLists.txt b/nebula_messages/nebula_msgs/CMakeLists.txt new file mode 100644 index 000000000..2ffa068a5 --- /dev/null +++ b/nebula_messages/nebula_msgs/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(nebula_msgs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/NebulaPacket.msg" + "msg/NebulaPackets.msg" + DEPENDENCIES + std_msgs + ) + +ament_package() diff --git a/nebula_messages/nebula_msgs/msg/NebulaPacket.msg b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg new file mode 100644 index 000000000..f6ba285d8 --- /dev/null +++ b/nebula_messages/nebula_msgs/msg/NebulaPacket.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +uint8[] data diff --git a/nebula_messages/nebula_msgs/msg/NebulaPackets.msg b/nebula_messages/nebula_msgs/msg/NebulaPackets.msg new file mode 100644 index 000000000..7aa8444f1 --- /dev/null +++ b/nebula_messages/nebula_msgs/msg/NebulaPackets.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +NebulaPacket[] packets diff --git a/nebula_messages/nebula_msgs/package.xml b/nebula_messages/nebula_msgs/package.xml new file mode 100644 index 000000000..146e14d07 --- /dev/null +++ b/nebula_messages/nebula_msgs/package.xml @@ -0,0 +1,25 @@ + + + + nebula_msgs + 0.1.0 + Generic sensor raw data messages for Nebula + Kenzo Lobos-Tsunekawa + + Apache 2 + Tier IV + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 954c81d04..fe6bc9f93 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -123,6 +123,26 @@ rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper EXECUTABLE robosense_hw_monitor_ros_wrapper_node ) +## Continental +# Hw Interface +ament_auto_add_library(continental_hw_ros_wrapper SHARED + src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(continental_hw_ros_wrapper + PLUGIN "ContinentalRadarEthernetHwInterfaceRosWrapper" + EXECUTABLE continental_hw_interface_ros_wrapper_node + ) + +# DriverDecoder +ament_auto_add_library(continental_driver_ros_wrapper SHARED + src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp + ) +rclcpp_components_register_node(continental_driver_ros_wrapper + PLUGIN "ContinentalRadarEthernetDriverRosWrapper" + EXECUTABLE continental_driver_ros_wrapper_node + ) + + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp index 3872146ab..694928ce4 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp @@ -1,3 +1,17 @@ +// Copyright 2023 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_DRIVER_WRAPPER_BASE_H #define NEBULA_DRIVER_WRAPPER_BASE_H @@ -10,6 +24,7 @@ #include +#include #include #include @@ -34,8 +49,12 @@ class NebulaDriverRosWrapperBase /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status virtual Status InitializeDriver( - std::shared_ptr sensor_configuration, - std::shared_ptr calibration_configuration) = 0; + [[maybe_unused]] std::shared_ptr sensor_configuration, + [[maybe_unused]] std::shared_ptr + calibration_configuration) + { + return Status::NOT_IMPLEMENTED; + } // status ReceiveScanMsgCallback(void * ScanMsg); // ROS message callback for individual packet // type diff --git a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp new file mode 100644 index 000000000..bf4776c90 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp @@ -0,0 +1,113 @@ +// Copyright 2023 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_ContinentalRadarEthernetDriverRosWrapper_H +#define NEBULA_ContinentalRadarEthernetDriverRosWrapper_H + +#include "nebula_common/continental/continental_common.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_common/nebula_status.hpp" +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp" +#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include +#include + +#include "continental_msgs/msg/continental_ars548_detection.hpp" +#include "continental_msgs/msg/continental_ars548_detection_list.hpp" +#include "continental_msgs/msg/continental_ars548_object.hpp" +#include "continental_msgs/msg/continental_ars548_object_list.hpp" +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Ros wrapper of continental radar ethernet driver +class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, + NebulaDriverRosWrapperBase +{ + std::shared_ptr decoder_ptr_; + Status wrapper_status_; + + rclcpp::Subscription::SharedPtr packets_sub_; + + rclcpp::Publisher::SharedPtr + detection_list_pub_; + rclcpp::Publisher::SharedPtr object_list_pub_; + rclcpp::Publisher::SharedPtr object_pointcloud_pub_; + rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; + + std::shared_ptr sensor_cfg_ptr_; + + drivers::ContinentalRadarEthernetHwInterface hw_interface_; + + /// @brief Initializing ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @param calibration_configuration CalibrationConfiguration for this driver + /// @return Resulting status + Status InitializeDriver(std::shared_ptr sensor_configuration); + + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @param calibration_configuration Output of CalibrationConfiguration + /// @param correction_configuration Output of CorrectionConfiguration (for AT) + /// @return Resulting status + Status GetParameters(drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration); + + /// @brief Convert seconds to chrono::nanoseconds + /// @param seconds + /// @return chrono::nanoseconds + static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + { + return std::chrono::duration_cast( + std::chrono::duration(seconds)); + } + + /*** + * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher + * @param pointcloud unique pointer containing the point cloud to publish + * @param publisher + */ + void PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + + void detectionListCallback( + std::unique_ptr msg); + void objectListCallback(std::unique_ptr msg); + +public: + explicit ContinentalRadarEthernetDriverRosWrapper(const rclcpp::NodeOptions & options); + + /// @brief Callback for NebulaPackets subscriber + /// @param scan_msg Received NebulaPackets + void ReceivePacketsMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + + /// @brief Get current status of this driver + /// @return Current status + Status GetStatus(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRadarEthernetDriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..ec0c9316b --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp @@ -0,0 +1,136 @@ +// Copyright 2023 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_ContinentalRadarEthernetHwInterfaceRosWrapper_H +#define NEBULA_ContinentalRadarEthernetHwInterfaceRosWrapper_H + +#include "boost_tcp_driver/tcp_driver.hpp" +#include "nebula_common/continental/continental_common.hpp" +#include "nebula_common/nebula_common.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp" +#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" + +#include +#include +#include +#include + +#include "nebula_msgs/msg/nebula_packet.hpp" +#include "nebula_msgs/msg/nebula_packets.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_srvs/srv/empty.hpp" + +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::ContinentalRadarEthernetHwInterface hw_interface_; + Status interface_status_; + + drivers::ContinentalRadarEthernetSensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + rclcpp::Publisher::SharedPtr packets_pub_; + rclcpp::Subscription::SharedPtr driving_direction_sub_; + rclcpp::Service::SharedPtr sensor_config_service_server_; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); + + /// @brief (Dummy) Callback to set the driving direction + /// @param msg A dummy msg + void DrivingDirectionCallback(const std_msgs::msg::Bool::SharedPtr msg); + + /// @brief (Dummy) Service callback to configure the sensor + /// @param request Empty service request + /// @param response Empty service response + void SensorConfigureRequestCallback( + const std::shared_ptr request, + const std::shared_ptr response); + +public: + explicit ContinentalRadarEthernetHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalRadarEthernetHwInterfaceRosWrapper() noexcept override; + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters(drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + diagnostic_updater::Updater diagnostics_updater_; + void ContinentalMonitorStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRadarEthernetHwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp index 0f0b02acc..81315a971 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #pragma once #include "nebula_common/nebula_common.hpp" @@ -18,6 +34,7 @@ #include "robosense_msgs/msg/robosense_scan.hpp" #include +#include namespace nebula { @@ -37,7 +54,7 @@ class RobosenseDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrap rclcpp::Publisher::SharedPtr aw_points_base_pub_; std::shared_ptr calibration_cfg_ptr_; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr sensor_cfg_ptr_; /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver @@ -96,4 +113,4 @@ class RobosenseDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrap }; } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp index 2e1155568..2095d75fb 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #pragma once #include "nebula_common/nebula_common.hpp" @@ -12,6 +28,11 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" +#include +#include +#include +#include + namespace nebula { namespace ros @@ -81,7 +102,8 @@ class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterf /// @brief Callback for receiving RobosensePacket /// @param difop_buffer Received DIFOP packet - void ReceiveInfoDataCallback(std::unique_ptr difop_buffer); + void ReceiveInfoDataCallback( + std::unique_ptr difop_buffer); }; } // namespace ros diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp index 2ecdc0b4f..80b8ff257 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #pragma once #include "boost_tcp_driver/tcp_driver.hpp" @@ -18,8 +34,12 @@ #include #include +#include +#include #include +#include #include +#include namespace nebula { diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml new file mode 100644 index 000000000..03e01aee0 --- /dev/null +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index c9c86d896..a0a394bc4 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -21,9 +21,10 @@ pcl_conversions rclcpp rclcpp_components - yaml-cpp - velodyne_msgs robosense_msgs + std_srvs + velodyne_msgs + yaml-cpp ament_cmake_gtest ament_lint_auto diff --git a/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..947314c38 --- /dev/null +++ b/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp @@ -0,0 +1,210 @@ +// Copyright 2023 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_radar_ethernet_decoder_ros_wrapper.hpp" + +#include "pcl_conversions/pcl_conversions.h" + +namespace nebula +{ +namespace ros +{ +ContinentalRadarEthernetDriverRosWrapper::ContinentalRadarEthernetDriverRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_driver_ros_wrapper", options), hw_interface_() +{ + drivers::ContinentalRadarEthernetSensorConfiguration sensor_configuration; + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + + 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); + + wrapper_status_ = + InitializeDriver(std::const_pointer_cast(sensor_cfg_ptr_)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalRadarEthernetDriverRosWrapper::ReceivePacketsMsgCallback, this, + std::placeholders::_1)); + + detection_list_pub_ = + this->create_publisher( + "detection_array", rclcpp::SensorDataQoS()); + object_list_pub_ = this->create_publisher( + "object_array", rclcpp::SensorDataQoS()); + + detection_pointcloud_pub_ = this->create_publisher( + "detection_points", rclcpp::SensorDataQoS()); + object_pointcloud_pub_ = + this->create_publisher("object_points", rclcpp::SensorDataQoS()); +} + +void ContinentalRadarEthernetDriverRosWrapper::ReceivePacketsMsgCallback( + const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) +{ + decoder_ptr_->ProcessPackets(*scan_msg); +} + +void ContinentalRadarEthernetDriverRosWrapper::PublishCloud( + std::unique_ptr pointcloud, + const rclcpp::Publisher::SharedPtr & publisher) +{ + if (pointcloud->header.stamp.sec < 0) { + RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); + } + pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; + publisher->publish(std::move(pointcloud)); +} + +Status ContinentalRadarEthernetDriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration) +{ + decoder_ptr_ = std::make_shared( + std::static_pointer_cast( + sensor_configuration)); + + decoder_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalRadarEthernetDriverRosWrapper::detectionListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterObjectListCallback(std::bind( + &ContinentalRadarEthernetDriverRosWrapper::objectListCallback, this, std::placeholders::_1)); + + return Status::OK; +} + +Status ContinentalRadarEthernetDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalRadarEthernetDriverRosWrapper::GetParameters( + drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", "255.255.255.255", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", 2368, descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "continental_ars548", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration); + + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalRadarEthernetDriverRosWrapper::detectionListCallback( + std::unique_ptr msg) +{ + if ( + detection_pointcloud_pub_->get_subscription_count() > 0 || + detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { + const auto detection_pointcloud_ptr = + nebula::drivers::continental_ars548::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; + detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + if ( + detection_list_pub_->get_subscription_count() > 0 || + detection_list_pub_->get_intra_process_subscription_count() > 0) { + detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalRadarEthernetDriverRosWrapper::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 = + nebula::drivers::continental_ars548::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 ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalRadarEthernetDriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..63352a6a3 --- /dev/null +++ b/nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp @@ -0,0 +1,349 @@ +// Copyright 2023 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_radar_ethernet_hw_interface_ros_wrapper.hpp" + +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalRadarEthernetHwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), + hw_interface_(), + diagnostics_updater_(this) +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + hw_interface_.RegisterScanCallback(std::bind( + &ContinentalRadarEthernetHwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1)); + packets_pub_ = this->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + +#if not defined(TEST_PCAP) // KL check how to do this + if (this->setup_sensor) { + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + } +#endif + + driving_direction_sub_ = create_subscription( + "driving_direction", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalRadarEthernetHwInterfaceRosWrapper::DrivingDirectionCallback, this, + std::placeholders::_1)); + + sensor_config_service_server_ = this->create_service( + "configure_radar", + std::bind( + &ContinentalRadarEthernetHwInterfaceRosWrapper::SensorConfigureRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + StreamStart(); +} + +ContinentalRadarEthernetHwInterfaceRosWrapper::~ContinentalRadarEthernetHwInterfaceRosWrapper() +{ +} + +Status ContinentalRadarEthernetHwInterfaceRosWrapper::StreamStart() +{ + using std::chrono_literals::operator""ms; + + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.CloudInterfaceStart(); + diagnostics_updater_.add( + "radar_status", this, + &ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalMonitorStatus); + } + + return interface_status_; +} + +Status ContinentalRadarEthernetHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status ContinentalRadarEthernetHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status ContinentalRadarEthernetHwInterfaceRosWrapper::GetParameters( + drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", "255.255.255.255", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", "224.0.0.1", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "continental", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", 2368, descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", 2368, descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", 2368, descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + if (sensor_configuration.frame_id.empty()) { + return Status::SENSOR_CONFIG_ERROR; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalRadarEthernetHwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer) +{ + packets_pub_->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult +ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::ContinentalRadarEthernetSensorConfiguration new_param{sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + std::string return_mode_str; + if ( + get_param(p, "sensor_model", sensor_model_str) || + get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || + get_param(p, "sensor_ip", new_param.sensor_ip) || + get_param(p, "frame_id", new_param.frame_id) || + get_param(p, "data_port", new_param.data_port) || + get_param(p, "configuration_host_port", new_param.configuration_host_port) || + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void ContinentalRadarEthernetHwInterfaceRosWrapper::DrivingDirectionCallback( + [[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) +{ + std::cout << std::this_thread::get_id() + << ": ContinentalRadarEthernetHwInterface::DrivingDirectionTimerCallback" << std::endl + << std::flush; + + hw_interface_.SetDrivingDirection(0); + hw_interface_.SetAccelerationLateralCog(0.0); + hw_interface_.SetAccelerationLongitudinalCog(0.0); + + hw_interface_.SetCharasteristicSpeed(0.0); + hw_interface_.SetSteeringAngleFrontAxle(0.0); + hw_interface_.SetVelocityVehicle(5.0); + hw_interface_.SetYawRate(0.25); +} + +void ContinentalRadarEthernetHwInterfaceRosWrapper::SensorConfigureRequestCallback( + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) +{ + std::cout << "SetVehicleParametersCallback" << std::endl << std::flush; + + hw_interface_.SetVehicleParameters(10.f, 4.5f, 3.f, 2.5f); +} + +std::vector +ContinentalRadarEthernetHwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), + rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +void ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalMonitorStatus( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +{ + std::cout << std::this_thread::get_id() << ": ContinentalMonitorStatus" << std::endl + << std::flush; + + auto sensor_status = hw_interface_.GetRadarStatus(); + // KL need to use smart pointers for the initial case + diagnostics.add("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); + diagnostics.add("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); + diagnostics.add("timestamp_sync_status", sensor_status.timestamp_sync_status); + diagnostics.add("sw_version_major", std::to_string(sensor_status.sw_version_major)); + diagnostics.add("sw_version_minor", std::to_string(sensor_status.sw_version_minor)); + diagnostics.add("sw_version_patch", std::to_string(sensor_status.sw_version_patch)); + diagnostics.add("longitudinal", std::to_string(sensor_status.longitudinal)); + diagnostics.add("lateral", std::to_string(sensor_status.lateral)); + diagnostics.add("vertical", std::to_string(sensor_status.vertical)); + diagnostics.add("yaw", std::to_string(sensor_status.yaw)); + diagnostics.add("pitch", std::to_string(sensor_status.pitch)); + diagnostics.add("plug_orientation", sensor_status.plug_orientation); + diagnostics.add("length", std::to_string(sensor_status.length)); + diagnostics.add("width", std::to_string(sensor_status.width)); + diagnostics.add("height", std::to_string(sensor_status.height)); + diagnostics.add("wheel_base", std::to_string(sensor_status.wheel_base)); + diagnostics.add("max_distance", std::to_string(sensor_status.max_distance)); + diagnostics.add("frequency_slot", sensor_status.frequency_slot); + diagnostics.add("cycle_time", std::to_string(sensor_status.cycle_time)); + diagnostics.add("time_slot", std::to_string(sensor_status.time_slot)); + diagnostics.add("hcc", sensor_status.hcc); + diagnostics.add("powersave_standstill", sensor_status.powersave_standstill); + diagnostics.add("sensor_ip_address0", sensor_status.sensor_ip_address0); + diagnostics.add("sensor_ip_address1", sensor_status.sensor_ip_address1); + diagnostics.add("configuration_counter", std::to_string(sensor_status.configuration_counter)); + diagnostics.add("status_longitudinal_velocity", sensor_status.status_longitudinal_velocity); + diagnostics.add( + "status_longitudinal_acceleration", sensor_status.status_longitudinal_acceleration); + diagnostics.add("status_lateral_acceleration", sensor_status.status_lateral_acceleration); + diagnostics.add("status_yaw_rate", sensor_status.status_yaw_rate); + diagnostics.add("status_steering_angle", sensor_status.status_steering_angle); + diagnostics.add("status_driving_direction", sensor_status.status_driving_direction); + diagnostics.add("charasteristic_speed", sensor_status.charasteristic_speed); + diagnostics.add("radar_status", sensor_status.radar_status); + diagnostics.add("voltage_status", sensor_status.voltage_status); + diagnostics.add("temperature_status", sensor_status.temperature_status); + diagnostics.add("blockage_status", sensor_status.blockage_status); + diagnostics.summary( + diagnostic_msgs::msg::DiagnosticStatus::WARN, + "Dummy No data available"); // KL: need to check this +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalRadarEthernetHwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index d779e3bc6..b7ab9e585 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -1,5 +1,20 @@ -#include "nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp" +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 +#include "nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp" namespace nebula { namespace ros @@ -58,7 +73,7 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( } if (!info_driver_ptr_) { wrapper_status_ = InitializeInfoDriver( - std::const_pointer_cast(sensor_cfg_ptr_)); + std::static_pointer_cast(sensor_cfg_ptr_)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } } @@ -77,7 +92,7 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; - }; + } if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { diff --git a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp index edac60133..c5a2eb06e 100644 --- a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #include "nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp" namespace nebula @@ -166,4 +182,4 @@ void RobosenseHwInterfaceRosWrapper::ReceiveInfoDataCallback( RCLCPP_COMPONENTS_REGISTER_NODE(RobosenseHwInterfaceRosWrapper) } // namespace ros -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp index 9818d10e5..5d562e096 100644 --- a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #include "nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp" #include From 7e3f2f7b4c258d080a566b9c7e93775340f3d5dc Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 13 Dec 2023 18:13:31 +0900 Subject: [PATCH 02/42] chore: refactor before sending PR Changed the generic naming to ARS548 since it looked like many things can not be generalized Added documentation to the new methods Addressed remaining TODOs Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental}/continental_ars548.hpp | 66 +++- .../continental/continental_common.hpp | 6 +- nebula_decoders/CMakeLists.txt | 1 - .../decoders/continental_ars548_decoder.hpp | 12 +- .../decoders/continental_packets_decoder.hpp | 2 +- .../decoders/continental_ars548.cpp | 175 --------- .../decoders/continental_ars548_decoder.cpp | 6 +- nebula_hw_interfaces/CMakeLists.txt | 2 +- ...pp => continental_ars548_hw_interface.hpp} | 128 +++---- .../continental_types.hpp | 14 +- ...pp => continental_ars548_hw_interface.cpp} | 96 ++--- nebula_ros/CMakeLists.txt | 4 +- ...ontinental_ars548_decoder_ros_wrapper.hpp} | 61 +++- ...ental_ars548_hw_interface_ros_wrapper.hpp} | 26 +- nebula_ros/package.xml | 1 + ...continental_ars548_decoder_ros_wrapper.cpp | 344 ++++++++++++++++++ ...ental_ars548_hw_interface_ros_wrapper.cpp} | 76 ++-- ...tal_radar_ethernet_decoder_ros_wrapper.cpp | 210 ----------- .../robosense_hw_monitor_ros_wrapper.cpp | 2 +- 19 files changed, 607 insertions(+), 625 deletions(-) rename {nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders => nebula_common/include/nebula_common/continental}/continental_ars548.hpp (74%) delete mode 100644 nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp rename nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/{continental_radar_ethernet_hw_interface.hpp => continental_ars548_hw_interface.hpp} (72%) rename nebula_hw_interfaces/src/nebula_continental_hw_interfaces/{continental_radar_ethernet_hw_interface.cpp => continental_ars548_hw_interface.cpp} (93%) rename nebula_ros/include/nebula_ros/continental/{continental_radar_ethernet_decoder_ros_wrapper.hpp => continental_ars548_decoder_ros_wrapper.hpp} (63%) rename nebula_ros/include/nebula_ros/continental/{continental_radar_ethernet_hw_interface_ros_wrapper.hpp => continental_ars548_hw_interface_ros_wrapper.hpp} (84%) create mode 100644 nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp rename nebula_ros/src/continental/{continental_radar_ethernet_hw_interface_ros_wrapper.cpp => continental_ars548_hw_interface_ros_wrapper.cpp} (81%) delete mode 100644 nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp similarity index 74% rename from nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp rename to nebula_common/include/nebula_common/continental/continental_ars548.hpp index f98f3ee20..fd68e721a 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -16,11 +16,6 @@ /** * Continental ARS548 */ -#include "continental_msgs/msg/continental_ars548_detection_list.hpp" -#include "continental_msgs/msg/continental_ars548_object_list.hpp" -#include "radar_msgs/msg/radar_scan.hpp" -#include "radar_msgs/msg/radar_tracks.hpp" - #include #include @@ -38,14 +33,62 @@ constexpr int SERVICE_ID_BYTE = 0; constexpr int METHOD_ID_BYTE = 2; constexpr int LENGTH_BYTE = 4; +constexpr int CONFIGURATION_METHOD_ID = 390; +constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; + +constexpr int STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; +constexpr int STATUS_TIMESTAMP_SECONDS_BYTE = 12; +constexpr int STATUS_SYNC_STATUS_BYTE = 16; +constexpr int STATUS_SW_VERSION_MAJOR_BYTE = 17; +constexpr int STATUS_SW_VERSION_MINOR_BYTE = 18; +constexpr int STATUS_SW_VERSION_PATCH_BYTE = 19; + +constexpr int STATUS_LONGITUDINAL_BYTE = 20; +constexpr int STATUS_LATERAL_BYTE = 24; +constexpr int STATUS_VERTICAL_BYTE = 28; +constexpr int STATUS_YAW_BYTE = 32; +constexpr int STATUS_PITCH_BYTE = 36; + +constexpr int STATUS_PLUG_ORIENTATION_BYTE = 40; +constexpr int STATUS_LENGTH_BYTE = 41; +constexpr int STATUS_WIDTH_BYTE = 45; +constexpr int STATUS_HEIGHT_BYTE = 49; +constexpr int STATUS_WHEEL_BASE_BYTE = 53; +constexpr int STATUS_MAXIMUM_DISTANCE_BYTE = 57; +constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; +constexpr int STATUS_CYCLE_TIME_BYTE = 60; +constexpr int STATUS_TIME_SLOT_BYTE = 61; +constexpr int STATUS_HCC_BYTE = 62; +constexpr int STATUS_POWERSAVING_STANDSTILL_BYTE = 63; +constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; +constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; +constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; +constexpr int STATUS_LONGITUDINAL_VELOCITY_BYTE = 73; +constexpr int STATUS_LONGITUDINAL_ACCELERATION_BYTE = 74; +constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; +constexpr int STATUS_YAW_RATE_BYTE = 76; +constexpr int STATUS_STEERING_ANGLE_BYTE = 77; +constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; +constexpr int STATUS_CHARASTERISTIC_SPEED_BYTE = 79; +constexpr int STATUS_RADAR_STATUS_BYTE = 80; +constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; +constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; +constexpr int STATUS_BLOCKAGE_STATUS_BYTE = 83; + constexpr int DETECTION_LIST_METHOD_ID = 336; constexpr int OBJECT_LIST_METHOD_ID = 329; +constexpr int SENSOR_STATUS_METHOD_ID = 380; +constexpr int FILTER_STATUS_METHOD_ID = 396; constexpr int DETECTION_LIST_UDP_PAYPLOAD = 35336; constexpr int OBJECT_LIST_UDP_PAYPLOAD = 9401; +constexpr int SENSOR_STATUS_UDP_PAYPLOAD = 84; +constexpr int FILTER_STATUS_UDP_PAYPLOAD = 330; constexpr int DETECTION_LIST_PDU_LENGTH = 35328; constexpr int OBJECT_LIST_PDU_LENGTH = 9393; +constexpr int SENSOR_STATUS_PDU_LENGTH = 76; +constexpr int FILTER_STATUS_PDU_LENGTH = 322; constexpr int DETECTION_LIST_CRC_BYTE = 16; constexpr int DETECTION_LIST_LENGTH_BYTE = 24; @@ -156,17 +199,8 @@ constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = 157; constexpr int OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = 166; constexpr int OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = 179; -pcl::PointCloud::Ptr convertToPointcloud( - const continental_msgs::msg::ContinentalArs548DetectionList & msg); - -pcl::PointCloud::Ptr convertToPointcloud( - const continental_msgs::msg::ContinentalArs548ObjectList & msg); - -radar_msgs::msg::RadarScan convertToRadarScan( - const continental_msgs::msg::ContinentalArs548DetectionList & msg); - -radar_msgs::msg::RadarTracks convertToRadarTracks( - const continental_msgs::msg::ContinentalArs548ObjectList & msg); +static constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; +static constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp index 3647bfeac..439cb2ead 100644 --- a/nebula_common/include/nebula_common/continental/continental_common.hpp +++ b/nebula_common/include/nebula_common/continental/continental_common.hpp @@ -30,20 +30,20 @@ namespace nebula namespace drivers { /// @brief struct for Hesai sensor configuration -struct ContinentalRadarEthernetSensorConfiguration : EthernetSensorConfigurationBase +struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; uint16_t configuration_host_port{}; uint16_t configuration_sensor_port{}; }; -/// @brief Convert ContinentalRadarEthernetSensorConfiguration to string (Overloading the << +/// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << /// operator) /// @param os /// @param arg /// @return stream inline std::ostream & operator<<( - std::ostream & os, ContinentalRadarEthernetSensorConfiguration const & arg) + std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip << ", ConfigurationHostPort: " << arg.configuration_host_port diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index 11485489b..283779b5f 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -53,7 +53,6 @@ ament_auto_add_library(nebula_decoders_robosense_info SHARED # Continental ament_auto_add_library(nebula_decoders_continental SHARED - src/nebula_decoders_continental/decoders/continental_ars548.cpp src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp ) 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 c7d9e394a..80e622017 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,7 +14,7 @@ #pragma once -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp" +#include "nebula_common/continental/continental_common.hpp" #include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" #include "nebula_msgs/msg/nebula_packet.hpp" @@ -39,8 +39,7 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder explicit ContinentalARS548Decoder( - const std::shared_ptr & - sensor_configuration); + const std::shared_ptr & sensor_configuration); /// @brief Function for parsing NebulaPackets /// @param nebula_packets @@ -50,9 +49,16 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder bool ParseDetectionsListPacket(const std::vector & data); bool ParseObjectsListPacket(const std::vector & data); + /// @brief Register function to call whenever a new detection list is processed + /// @param detection_list_callback + /// @return Resulting status Status RegisterDetectionListCallback( 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); 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 f7867aeb1..7342e2b0b 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 @@ -33,7 +33,7 @@ class ContinentalPacketsDecoder { protected: /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; public: ContinentalPacketsDecoder(ContinentalPacketsDecoder && c) = delete; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp deleted file mode 100644 index 88b1e7f82..000000000 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548.cpp +++ /dev/null @@ -1,175 +0,0 @@ -// Copyright 2023 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_ars548.hpp" - -namespace nebula -{ -namespace drivers -{ -namespace continental_ars548 -{ - -pcl::PointCloud::Ptr convertToPointcloud( - const continental_msgs::msg::ContinentalArs548DetectionList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); - output_pointcloud->reserve(msg.detections.size()); - - pcl::PointXYZ point{}; - for (const auto & detection : msg.detections) { - point.x = - std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; - point.y = - std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; - point.z = std::sin(detection.elevation_angle) * detection.range; - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -pcl::PointCloud::Ptr convertToPointcloud( - const continental_msgs::msg::ContinentalArs548ObjectList & msg) -{ - pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); - output_pointcloud->reserve(msg.objects.size()); - - pcl::PointXYZ point{}; - for (const auto & detection : msg.objects) { - point.x = static_cast(detection.position.x); - point.y = static_cast(detection.position.y); - point.z = static_cast(detection.position.z); - - output_pointcloud->points.emplace_back(point); - } - - output_pointcloud->height = 1; - output_pointcloud->width = output_pointcloud->points.size(); - return output_pointcloud; -} - -radar_msgs::msg::RadarScan convertToRadarScan( - const continental_msgs::msg::ContinentalArs548DetectionList & 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.invalid_azimuth || detection.invalid_distance || detection.invalid_elevation || - detection.invalid_range_rate) { - continue; - } - - return_msg.range = detection.range; - return_msg.azimuth = detection.azimuth_angle; - return_msg.elevation = detection.elevation_angle; - 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 convertToRadarTracks( - const continental_msgs::msg::ContinentalArs548ObjectList & 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 int16_t CAR_ID = 32001; - constexpr int16_t TRUCK_ID = 32002; - constexpr int16_t MOTORCYCLE_ID = 32005; - constexpr int16_t BICYCLE_ID = 32006; - constexpr int16_t PEDESTRIAN_ID = 32007; - - radar_msgs::msg::RadarTrack track_msg; - for (const auto & detection : msg.objects) { - track_msg.uuid.uuid[0] = static_cast(detection.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((detection.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((detection.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((detection.object_id >> 24) & 0xff); - track_msg.position = detection.position; - track_msg.velocity = detection.absolute_velocity; - track_msg.acceleration = detection.absolute_acceleration; - track_msg.size.x = detection.shape_length_edge_mean; - track_msg.size.y = detection.shape_width_edge_mean; - track_msg.size.z = 1.f; - - uint8_t max_score = detection.classification_unknown; - track_msg.classification = UNKNOWN_ID; - - if (detection.classification_car > max_score) { - max_score = detection.classification_car; - track_msg.classification = CAR_ID; - } - if (detection.classification_truck > max_score) { - max_score = detection.classification_truck; - track_msg.classification = TRUCK_ID; - } - if (detection.classification_motorcycle > max_score) { - max_score = detection.classification_motorcycle; - track_msg.classification = MOTORCYCLE_ID; - } - if (detection.classification_bicycle > max_score) { - max_score = detection.classification_bicycle; - track_msg.classification = BICYCLE_ID; - } - if (detection.classification_pedestrian > max_score) { - max_score = detection.classification_pedestrian; - track_msg.classification = PEDESTRIAN_ID; - } - - track_msg.position_covariance[0] = static_cast(detection.position_std.x); - track_msg.position_covariance[1] = detection.position_covariance_xy; - track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = static_cast(detection.position_std.y); - track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = static_cast(detection.position_std.z); - - track_msg.velocity_covariance[0] = static_cast(detection.absolute_velocity_std.x); - track_msg.velocity_covariance[1] = detection.absolute_velocity_covariance_xy; - track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = static_cast(detection.absolute_velocity_std.y); - track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = static_cast(detection.absolute_velocity_std.z); - - track_msg.acceleration_covariance[0] = - static_cast(detection.absolute_acceleration_std.x); - track_msg.acceleration_covariance[1] = detection.absolute_acceleration_covariance_xy; - track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = - static_cast(detection.absolute_acceleration_std.y); - track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = - static_cast(detection.absolute_acceleration_std.z); - - output_msg.tracks.emplace_back(track_msg); - } - - return output_msg; -} - -} // namespace continental_ars548 -} // 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 0e3e2fe70..99b6a7ba7 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 @@ -14,7 +14,8 @@ #include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548.hpp" +#include "nebula_common/continental/continental_ars548.hpp" +#include "nebula_common/continental/continental_common.hpp" #include #include @@ -26,8 +27,7 @@ namespace drivers namespace continental_ars548 { ContinentalARS548Decoder::ContinentalARS548Decoder( - const std::shared_ptr & - sensor_configuration) + const std::shared_ptr & sensor_configuration) { sensor_configuration_ = sensor_configuration; } diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 367e52362..7cd8b8982 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -39,7 +39,7 @@ ament_auto_add_library(nebula_hw_interfaces_robosense SHARED ) ament_auto_add_library(nebula_hw_interfaces_continental SHARED - src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp + src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp similarity index 72% rename from nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp rename to nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index a9879cdcc..c0192a387 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -27,6 +27,7 @@ #include "boost_tcp_driver/http_client_driver.hpp" #include "boost_tcp_driver/tcp_driver.hpp" #include "boost_udp_driver/udp_driver.hpp" +#include "nebula_common/continental/continental_ars548.hpp" #include "nebula_common/continental/continental_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp" @@ -49,69 +50,21 @@ namespace nebula { namespace drivers { -constexpr int SERVICE_ID_BYTE = 0; -constexpr int METHOD_ID_BYTE = 2; -constexpr int LENGTH_BYTE = 4; - -constexpr int CONFIGURATION_METHOD_ID = 390; -constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; - -constexpr int STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; -constexpr int STATUS_TIMESTAMP_SECONDS_BYTE = 12; -constexpr int STATUS_SYNC_STATUS_BYTE = 16; -constexpr int STATUS_SW_VERSION_MAJOR_BYTE = 17; -constexpr int STATUS_SW_VERSION_MINOR_BYTE = 18; -constexpr int STATUS_SW_VERSION_PATCH_BYTE = 19; - -constexpr int STATUS_LONGITUDINAL_BYTE = 20; -constexpr int STATUS_LATERAL_BYTE = 24; -constexpr int STATUS_VERTICAL_BYTE = 28; -constexpr int STATUS_YAW_BYTE = 32; -constexpr int STATUS_PITCH_BYTE = 36; - -constexpr int STATUS_PLUG_ORIENTATION_BYTE = 40; -constexpr int STATUS_LENGTH_BYTE = 41; -constexpr int STATUS_WIDTH_BYTE = 45; -constexpr int STATUS_HEIGHT_BYTE = 49; -constexpr int STATUS_WHEEL_BASE_BYTE = 53; -constexpr int STATUS_MAXIMUM_DISTANCE_BYTE = 57; -constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; -constexpr int STATUS_CYCLE_TIME_BYTE = 60; -constexpr int STATUS_TIME_SLOT_BYTE = 61; -constexpr int STATUS_HCC_BYTE = 62; -constexpr int STATUS_POWERSAVING_STANDSTILL_BYTE = 63; -constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; -constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; -constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; -constexpr int STATUS_LONGITUDINAL_VELOCITY_BYTE = 73; -constexpr int STATUS_LONGITUDINAL_ACCELERATION_BYTE = 74; -constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; -constexpr int STATUS_YAW_RATE_BYTE = 76; -constexpr int STATUS_STEERING_ANGLE_BYTE = 77; -constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; -constexpr int STATUS_CHARASTERISTIC_SPEED_BYTE = 79; -constexpr int STATUS_RADAR_STATUS_BYTE = 80; -constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; -constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; -constexpr int STATUS_BLOCKAGE_STATUS_BYTE = 83; - -/// @brief Hardware interface of hesai driver -class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase +namespace continental_ars548 +{ +/// @brief Hardware interface of the Continental ARS548 radar +class ContinentalARS548HwInterface : NebulaHwInterfaceBase { private: std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; - std::shared_ptr sensor_configuration_; + std::shared_ptr sensor_configuration_; std::unique_ptr nebula_packets_ptr_; std::function buffer)> nebula_packets_reception_callback_; std::mutex sensor_status_mutex_; - // KL move these to the continental header file - static constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; - static constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; - struct FilterStatus { uint8_t active; @@ -123,67 +76,80 @@ class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase FilterStatus detection_filters_status_[DETECTION_FILTER_PROPERTIES_NUM]; FilterStatus object_filters_status_[OBJECT_FILTER_PROPERTIES_NUM]; - ContinentalRadarStatus radar_status_; + ContinentalARS548Status radar_status_; std::shared_ptr parent_node_logger; + /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string void PrintInfo(std::string info); + /// @brief Printing the string to RCLCPP_ERROR_STREAM /// @param error Target string void PrintError(std::string error); + /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string void PrintDebug(std::string debug); + /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM /// @param bytes Target byte vector void PrintDebug(const std::vector & bytes); public: /// @brief Constructor - ContinentalRadarEthernetHwInterface(); + ContinentalARS548HwInterface(); /// @brief Initializing tcp_driver for TCP communication /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration /// @return Resulting status + /// @brief Process a new sensor status packet + /// @param buffer The buffer containing the status packet void ProcessSensorStatusPacket(const std::vector & buffer); + + /// @brief Process a new filter status packet + /// @param buffer The buffer containing the status packet void ProcessFilterStatusPacket(const std::vector & buffer); - void ProcessDataPacket(const std::vector & buffer); - /// @brief Parsing json string to property_tree - /// @param str JSON string - /// @return Parsed property_tree - boost::property_tree::ptree ParseJson(const std::string & str); + /// @brief Process a new data packet + /// @param buffer The buffer containing the data packet + void ProcessDataPacket(const std::vector & buffer); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket void ReceiveCloudPacketCallbackWithSender( const std::vector & buffer, const std::string & sender_ip); + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket void ReceiveCloudPacketCallback(const std::vector & buffer) final; + /// @brief Starting the interface that handles UDP streams /// @return Resulting status Status CloudInterfaceStart() final; + /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status Status CloudInterfaceStop() final; + /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status Status SetSensorConfiguration( std::shared_ptr sensor_configuration) final; + /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status Status RegisterScanCallback( std::function)> scan_callback); - /// @brief @brief Setting SensorMounting + /// @brief Set the sensor mounting parameters /// @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 @@ -195,7 +161,7 @@ class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation); - /// @brief @brief Setting SensorMounting + /// @brief Set the vehicle parameters /// @param length_autosar Desired vehicle length value /// @param width_autosar Desired vehicle width value /// @param height_autosar Desired height value @@ -204,7 +170,7 @@ class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase Status SetVehicleParameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar); - /// @brief @brief Setting RadarParameters + /// @brief Set the radar parameters /// @param maximum_distance Desired maximum detection distance (93m <= v <= 1514m) /// @param frequency_slot Desired frequency slot (0 = Low (76.23 GHz), 1 = Mid (76.48 GHz), 2 = /// High (76.73 GHz)) @@ -217,22 +183,45 @@ class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t powersave_standstill); - /// @brief @brief Setting RadarParameters + /// @brief Set the sensor ip address /// @param sensor_ip_address Desired sensor ip address - /// @param with_run Automatically executes run() of TcpDriver /// @return Resulting status Status SetSensorIPAddress(const std::string & sensor_ip_address); + /// @brief Set the current lateral acceleration + /// @param lateral_acceleration Current lateral acceleration + /// @return Resulting status Status SetAccelerationLateralCog(float lateral_acceleration); + + /// @brief Set the current longitudinal acceleration + /// @param lonitudinal_acceleration Current longitudinal acceleration + /// @return Resulting status Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); + + /// @brief Set the charasteristic speed + /// @param charasteristic_speed Charasteristic speed + /// @return Resulting status Status SetCharasteristicSpeed(float charasteristic_speed); + + /// @brief Set the current direction + /// @param direction Current driving direction + /// @return Resulting status Status SetDrivingDirection(int direction); + + /// @brief Set the current steering angle + /// @param angle_rad Current steering angle in radians + /// @return Resulting status Status SetSteeringAngleFrontAxle(float angle_rad); + + /// @brief Set the current vehicle velocity + /// @param velocity Current vehicle velocity + /// @return Resulting status Status SetVelocityVehicle(float velocity); - Status SetYawRate(float yaw_rate); - Status GetLidarMonitor(std::shared_ptr ctx, bool with_run = true); - Status GetLidarMonitor(bool with_run = true); + /// @brief Set the current yaw rate + /// @param yaw_rate Current yaw rate + /// @return Resulting status + Status SetYawRate(float yaw_rate); /// @brief Checking the current settings and changing the difference point /// @return Resulting status @@ -240,12 +229,13 @@ class ContinentalRadarEthernetHwInterface : NebulaHwInterfaceBase /// @brief Returns the last semantic sensor status /// @return Last semantic sensor status message - ContinentalRadarStatus GetRadarStatus(); + ContinentalARS548Status GetRadarStatus(); /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); }; +} // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index b3f3646b0..b4cd34944 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -24,8 +24,8 @@ namespace nebula { -/// @brief semantic struct of Radar Status (overfitted to ARS548) -struct ContinentalRadarStatus +/// @brief semantic struct of ARS548 Status +struct ContinentalARS548Status { uint32_t timestamp_nanoseconds; uint32_t timestamp_seconds; @@ -64,9 +64,13 @@ struct ContinentalRadarStatus std::string temperature_status; std::string blockage_status; - ContinentalRadarStatus() {} + ContinentalARS548Status() {} - friend std::ostream & operator<<(std::ostream & os, nebula::ContinentalRadarStatus const & arg) + /// @brief Stream ContinentalRadarStatus method + /// @param os + /// @param arg + /// @return stream + friend std::ostream & operator<<(std::ostream & os, nebula::ContinentalARS548Status const & arg) { os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; os << ", "; @@ -145,4 +149,4 @@ struct ContinentalRadarStatus }; } // namespace nebula -#endif // HESAI_CMD_RESPONSE_HPP +#endif // CONTINENTAL_TYPES_HPP diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp similarity index 93% rename from nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp rename to nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 878ad25c0..ccd9503b7 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_radar_ethernet_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -12,7 +12,10 @@ // 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_radar_ethernet_hw_interface.hpp" + +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" + +#include "nebula_common/continental/continental_ars548.hpp" #include @@ -20,21 +23,23 @@ namespace nebula { namespace drivers { -ContinentalRadarEthernetHwInterface::ContinentalRadarEthernetHwInterface() +namespace continental_ars548 +{ +ContinentalARS548HwInterface::ContinentalARS548HwInterface() : cloud_io_context_{new ::drivers::common::IoContext(1)}, sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, nebula_packets_ptr_{std::make_unique()} { } -Status ContinentalRadarEthernetHwInterface::SetSensorConfiguration( +Status ContinentalARS548HwInterface::SetSensorConfiguration( std::shared_ptr sensor_configuration) { Status status = Status::OK; try { sensor_configuration_ = - std::static_pointer_cast(sensor_configuration); + std::static_pointer_cast(sensor_configuration); } catch (const std::exception & ex) { status = Status::SENSOR_CONFIG_ERROR; std::cerr << status << std::endl; @@ -44,10 +49,9 @@ Status ContinentalRadarEthernetHwInterface::SetSensorConfiguration( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::CloudInterfaceStart() +Status ContinentalARS548HwInterface::CloudInterfaceStart() { try { - std::cout << "Starting Data UDP server on: " << *sensor_configuration_ << std::endl; sensor_udp_driver_->init_receiver( sensor_configuration_->multicast_ip, sensor_configuration_->data_port, sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); @@ -55,7 +59,7 @@ Status ContinentalRadarEthernetHwInterface::CloudInterfaceStart() sensor_udp_driver_->receiver()->open(); sensor_udp_driver_->receiver()->bind(); sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( - &ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallbackWithSender, this, + &ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); sensor_udp_driver_->init_sender( @@ -77,24 +81,23 @@ Status ContinentalRadarEthernetHwInterface::CloudInterfaceStart() return Status::OK; } -Status ContinentalRadarEthernetHwInterface::RegisterScanCallback( +Status ContinentalARS548HwInterface::RegisterScanCallback( std::function)> callback) { nebula_packets_reception_callback_ = std::move(callback); return Status::OK; } -void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallbackWithSender( +void ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender( const std::vector & buffer, const std::string & sender_ip) { if (sender_ip == sensor_configuration_->sensor_ip) { ReceiveCloudPacketCallback(buffer); } } -void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallback( - const std::vector & buffer) +void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) { - constexpr int DETECTION_LIST_METHOD_ID = 336; + /*constexpr int DETECTION_LIST_METHOD_ID = 336; constexpr int OBJECT_LIST_METHOD_ID = 329; constexpr int SENSOR_STATUS_METHOD_ID = 380; constexpr int FILTER_STATUS_METHOD_ID = 396; @@ -109,6 +112,8 @@ void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallback( constexpr int SENSOR_STATUS_PDU_LENGTH = 76; constexpr int FILTER_STATUS_PDU_LENGTH = 322; + */ + if (buffer.size() < LENGTH_BYTE + sizeof(uint32_t)) { PrintError("Unrecognized packet. Too short"); return; @@ -156,8 +161,7 @@ void ContinentalRadarEthernetHwInterface::ReceiveCloudPacketCallback( } } -void ContinentalRadarEthernetHwInterface::ProcessSensorStatusPacket( - const std::vector & buffer) +void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { radar_status_.timestamp_nanoseconds = (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | @@ -403,8 +407,7 @@ void ContinentalRadarEthernetHwInterface::ProcessSensorStatusPacket( } } -void ContinentalRadarEthernetHwInterface::ProcessFilterStatusPacket( - const std::vector & buffer) +void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) { // Unused available data // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; @@ -474,7 +477,7 @@ void ContinentalRadarEthernetHwInterface::ProcessFilterStatusPacket( } } -void ContinentalRadarEthernetHwInterface::ProcessDataPacket(const std::vector & buffer) +void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) { nebula_msgs::msg::NebulaPacket nebula_packet; nebula_packet.data = buffer; @@ -494,12 +497,12 @@ void ContinentalRadarEthernetHwInterface::ProcessDataPacket(const std::vector(); } -Status ContinentalRadarEthernetHwInterface::CloudInterfaceStop() +Status ContinentalARS548HwInterface::CloudInterfaceStop() { return Status::ERROR_1; } -Status ContinentalRadarEthernetHwInterface::GetSensorConfiguration( +Status ContinentalARS548HwInterface::GetSensorConfiguration( SensorConfigurationBase & sensor_configuration) { std::stringstream ss; @@ -508,18 +511,7 @@ Status ContinentalRadarEthernetHwInterface::GetSensorConfiguration( return Status::ERROR_1; } -boost::property_tree::ptree ContinentalRadarEthernetHwInterface::ParseJson(const std::string & str) -{ - boost::property_tree::ptree tree; - try { - boost::property_tree::read_json(str, tree); - } catch (boost::property_tree::json_parser_error & e) { - std::cerr << e.what() << std::endl; - } - return tree; -} - -Status ContinentalRadarEthernetHwInterface::SetSensorMounting( +Status ContinentalARS548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) { @@ -592,7 +584,7 @@ Status ContinentalRadarEthernetHwInterface::SetSensorMounting( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetVehicleParameters( +Status ContinentalARS548HwInterface::SetVehicleParameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) { constexpr int CONFIGURATION_LENGTH_BYTE = 29; @@ -653,7 +645,7 @@ Status ContinentalRadarEthernetHwInterface::SetVehicleParameters( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetRadarParameters( +Status ContinentalARS548HwInterface::SetRadarParameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t powersave_standstill) { @@ -699,8 +691,7 @@ Status ContinentalRadarEthernetHwInterface::SetRadarParameters( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetSensorIPAddress( - const std::string & sensor_ip_address) +Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) { constexpr int CONFIGURATION_SENSOR_IP_ADDRESS0 = 52; constexpr int CONFIGURATION_SENSOR_IP_ADDRESS1 = 56; @@ -742,7 +733,7 @@ Status ContinentalRadarEthernetHwInterface::SetSensorIPAddress( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetAccelerationLateralCog(float lateral_acceleration) +Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) { constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; @@ -758,7 +749,6 @@ Status ContinentalRadarEthernetHwInterface::SetAccelerationLateralCog(float late std::memcpy(bytes, &lateral_acceleration, sizeof(lateral_acceleration)); std::vector send_vector(ACCELERATION_LATERAL_COG_PAYLOAD_SIZE, 0); - ; send_vector[1] = ACCELERATION_LATERAL_COG_SERVICE_ID; send_vector[2] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID >> 8); send_vector[3] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID & 0x00ff); @@ -773,8 +763,7 @@ Status ContinentalRadarEthernetHwInterface::SetAccelerationLateralCog(float late return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetAccelerationLongitudinalCog( - float longitudinal_acceleration) +Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) { constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; @@ -790,7 +779,6 @@ Status ContinentalRadarEthernetHwInterface::SetAccelerationLongitudinalCog( std::memcpy(bytes, &longitudinal_acceleration, sizeof(longitudinal_acceleration)); std::vector send_vector(ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE, 0); - ; send_vector[1] = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; send_vector[2] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID >> 8); send_vector[3] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID & 0x00ff); @@ -805,7 +793,7 @@ Status ContinentalRadarEthernetHwInterface::SetAccelerationLongitudinalCog( return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetCharasteristicSpeed(float charasteristic_speed) +Status ContinentalARS548HwInterface::SetCharasteristicSpeed(float charasteristic_speed) { constexpr uint16_t CHARASTERISTIC_SPEED_SERVICE_ID = 0; constexpr uint16_t CHARASTERISTIC_SPEED_METHOD_ID = 328; @@ -818,7 +806,6 @@ Status ContinentalRadarEthernetHwInterface::SetCharasteristicSpeed(float charast } std::vector send_vector(CHARASTERISTIC_SPEED_PAYLOAD_SIZE, 0); - ; send_vector[1] = CHARASTERISTIC_SPEED_SERVICE_ID; send_vector[2] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID >> 8); send_vector[3] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID & 0x00ff); @@ -830,7 +817,7 @@ Status ContinentalRadarEthernetHwInterface::SetCharasteristicSpeed(float charast return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetDrivingDirection(int direction) +Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) { constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; @@ -841,7 +828,6 @@ Status ContinentalRadarEthernetHwInterface::SetDrivingDirection(int direction) const int DRIVING_DIRECTION_PAYLOAD_SIZE = DRIVING_DIRECTION_LENGTH + 8; std::vector send_vector(DRIVING_DIRECTION_PAYLOAD_SIZE, 0); - ; send_vector[1] = DRIVING_DIRECTION_SERVICE_ID; send_vector[2] = static_cast(DRIVING_DIRECTION_METHOD_ID >> 8); send_vector[3] = static_cast(DRIVING_DIRECTION_METHOD_ID & 0xff); @@ -860,7 +846,7 @@ Status ContinentalRadarEthernetHwInterface::SetDrivingDirection(int direction) return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetSteeringAngleFrontAxle(float angle_rad) +Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) { constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; @@ -876,7 +862,6 @@ Status ContinentalRadarEthernetHwInterface::SetSteeringAngleFrontAxle(float angl std::memcpy(bytes, &angle_rad, sizeof(angle_rad)); std::vector send_vector(STEERING_ANGLE_PAYLOAD_SIZE, 0); - ; send_vector[1] = STEERING_ANGLE_SERVICE_ID; send_vector[2] = static_cast(STEERING_ANGLE_METHOD_ID >> 8); send_vector[3] = static_cast(STEERING_ANGLE_METHOD_ID & 0x00ff); @@ -891,7 +876,7 @@ Status ContinentalRadarEthernetHwInterface::SetSteeringAngleFrontAxle(float angl return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetVelocityVehicle(float velocity) +Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) { constexpr uint16_t VELOCITY_VECHILE_SERVICE_ID = 0; constexpr uint16_t VELOCITY_VECHILE_METHOD_ID = 323; @@ -902,7 +887,6 @@ Status ContinentalRadarEthernetHwInterface::SetVelocityVehicle(float velocity) std::memcpy(bytes, &velocity, sizeof(velocity)); std::vector send_vector(VELOCITY_VECHILE_PAYLOAD_SIZE, 0); - ; send_vector[1] = VELOCITY_VECHILE_SERVICE_ID; send_vector[2] = static_cast(VELOCITY_VECHILE_METHOD_ID >> 8); send_vector[3] = static_cast(VELOCITY_VECHILE_METHOD_ID & 0x00ff); @@ -917,7 +901,7 @@ Status ContinentalRadarEthernetHwInterface::SetVelocityVehicle(float velocity) return Status::OK; } -Status ContinentalRadarEthernetHwInterface::SetYawRate(float yaw_rate) +Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) { constexpr uint16_t YAW_RATE_SERVICE_ID = 0; constexpr uint16_t YAW_RATE_METHOD_ID = 326; @@ -928,7 +912,6 @@ Status ContinentalRadarEthernetHwInterface::SetYawRate(float yaw_rate) std::memcpy(bytes, &yaw_rate, sizeof(yaw_rate)); std::vector send_vector(YAW_RATE_PAYLOAD_SIZE, 0); - ; send_vector[1] = YAW_RATE_SERVICE_ID; send_vector[2] = static_cast(YAW_RATE_METHOD_ID >> 8); send_vector[3] = static_cast(YAW_RATE_METHOD_ID & 0x00ff); @@ -943,18 +926,18 @@ Status ContinentalRadarEthernetHwInterface::SetYawRate(float yaw_rate) return Status::OK; } -ContinentalRadarStatus ContinentalRadarEthernetHwInterface::GetRadarStatus() +ContinentalARS548Status ContinentalARS548HwInterface::GetRadarStatus() { std::lock_guard l(sensor_status_mutex_); return radar_status_; } -void ContinentalRadarEthernetHwInterface::SetLogger(std::shared_ptr logger) +void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; } -void ContinentalRadarEthernetHwInterface::PrintInfo(std::string info) +void ContinentalARS548HwInterface::PrintInfo(std::string info) { if (parent_node_logger) { RCLCPP_INFO_STREAM((*parent_node_logger), info); @@ -963,7 +946,7 @@ void ContinentalRadarEthernetHwInterface::PrintInfo(std::string info) } } -void ContinentalRadarEthernetHwInterface::PrintError(std::string error) +void ContinentalARS548HwInterface::PrintError(std::string error) { if (parent_node_logger) { RCLCPP_ERROR_STREAM((*parent_node_logger), error); @@ -972,7 +955,7 @@ void ContinentalRadarEthernetHwInterface::PrintError(std::string error) } } -void ContinentalRadarEthernetHwInterface::PrintDebug(std::string debug) +void ContinentalARS548HwInterface::PrintDebug(std::string debug) { if (parent_node_logger) { RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); @@ -981,7 +964,7 @@ void ContinentalRadarEthernetHwInterface::PrintDebug(std::string debug) } } -void ContinentalRadarEthernetHwInterface::PrintDebug(const std::vector & bytes) +void ContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) { std::stringstream ss; for (const auto & b : bytes) { @@ -991,5 +974,6 @@ void ContinentalRadarEthernetHwInterface::PrintDebug(const std::vector PrintDebug(ss.str()); } +} // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index fe6bc9f93..fea064177 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -126,7 +126,7 @@ rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper ## Continental # Hw Interface ament_auto_add_library(continental_hw_ros_wrapper SHARED - src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp + src/continental/continental_ars548_hw_interface_ros_wrapper.cpp ) rclcpp_components_register_node(continental_hw_ros_wrapper PLUGIN "ContinentalRadarEthernetHwInterfaceRosWrapper" @@ -135,7 +135,7 @@ rclcpp_components_register_node(continental_hw_ros_wrapper # DriverDecoder ament_auto_add_library(continental_driver_ros_wrapper SHARED - src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp + src/continental/continental_ars548_decoder_ros_wrapper.cpp ) rclcpp_components_register_node(continental_driver_ros_wrapper PLUGIN "ContinentalRadarEthernetDriverRosWrapper" diff --git a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp similarity index 63% rename from nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp index bf4776c90..1f9fcc4ec 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalRadarEthernetDriverRosWrapper_H -#define NEBULA_ContinentalRadarEthernetDriverRosWrapper_H +#ifndef NEBULA_ContinentalARS548DriverRosWrapper_H +#define NEBULA_ContinentalARS548DriverRosWrapper_H #include "nebula_common/continental/continental_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" #include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" #include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" #include @@ -33,6 +33,8 @@ #include "continental_msgs/msg/continental_ars548_object_list.hpp" #include "nebula_msgs/msg/nebula_packet.hpp" #include "nebula_msgs/msg/nebula_packets.hpp" +#include "radar_msgs/msg/radar_scan.hpp" +#include "radar_msgs/msg/radar_tracks.hpp" #include #include @@ -42,8 +44,7 @@ namespace nebula namespace ros { /// @brief Ros wrapper of continental radar ethernet driver -class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, - NebulaDriverRosWrapperBase +class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperBase { std::shared_ptr decoder_ptr_; Status wrapper_status_; @@ -58,7 +59,7 @@ class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, std::shared_ptr sensor_cfg_ptr_; - drivers::ContinentalRadarEthernetHwInterface hw_interface_; + drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; /// @brief Initializing ros wrapper /// @param sensor_configuration SensorConfiguration for this driver @@ -71,7 +72,7 @@ class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, /// @param calibration_configuration Output of CalibrationConfiguration /// @param correction_configuration Output of CorrectionConfiguration (for AT) /// @return Resulting status - Status GetParameters(drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration); + Status GetParameters(drivers::ContinentalARS548SensorConfiguration & sensor_configuration); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds @@ -82,21 +83,17 @@ class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, std::chrono::duration(seconds)); } - /*** - * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher - * @param pointcloud unique pointer containing the point cloud to publish - * @param publisher - */ - void PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher); - - void detectionListCallback( + /// @brief Callback to process new ContinentalArs548DetectionList from the driver + /// @param msg The new ContinentalArs548DetectionList from the driver + void DetectionListCallback( std::unique_ptr msg); - void objectListCallback(std::unique_ptr msg); + + /// @brief Callback to process new ContinentalArs548ObjectList from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void ObjectListCallback(std::unique_ptr msg); public: - explicit ContinentalRadarEthernetDriverRosWrapper(const rclcpp::NodeOptions & options); + explicit ContinentalARS548DriverRosWrapper(const rclcpp::NodeOptions & options); /// @brief Callback for NebulaPackets subscriber /// @param scan_msg Received NebulaPackets @@ -105,9 +102,33 @@ class ContinentalRadarEthernetDriverRosWrapper final : public rclcpp::Node, /// @brief Get current status of this driver /// @return Current status Status GetStatus(); + + /// @brief Convert ARS548 detections to a pointcloud + /// @param msg The ARS548 detection list msg + /// @return Resulting detection pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548DetectionList & msg); + + /// @brief Convert ARS548 objects to a pointcloud + /// @param msg The ARS548 object list msg + /// @return Resulting object pointcloud + pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); + + /// @brief Convert ARS548 detections to a standard RadarScan msg + /// @param msg The ARS548 detection list msg + /// @return Resulting RadarScan msg + radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & msg); + + /// @brief Convert ARS548 objects to a standard RadarTracks msg + /// @param msg The ARS548 object list msg + /// @return Resulting RadarTracks msg + radar_msgs::msg::RadarTracks ConvertToRadarTracks( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); }; } // namespace ros } // namespace nebula -#endif // NEBULA_ContinentalRadarEthernetDriverRosWrapper_H +#endif // NEBULA_ContinentalARS548DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp similarity index 84% rename from nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp rename to nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp index ec0c9316b..f95882685 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_ContinentalRadarEthernetHwInterfaceRosWrapper_H -#define NEBULA_ContinentalRadarEthernetHwInterfaceRosWrapper_H +#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H #include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/continental/continental_common.hpp" #include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_radar_ethernet_hw_interface.hpp" +#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" #include @@ -63,13 +63,13 @@ bool get_param(const std::vector & p, const std::string & nam } /// @brief Hardware interface ros wrapper of continental radar ethernet driver -class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, - NebulaHwInterfaceWrapperBase +class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase { - drivers::ContinentalRadarEthernetHwInterface hw_interface_; + drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; Status interface_status_; - drivers::ContinentalRadarEthernetSensorConfiguration sensor_configuration_; + drivers::ContinentalARS548SensorConfiguration sensor_configuration_; /// @brief Received Continental Radar message publisher rclcpp::Publisher::SharedPtr packets_pub_; @@ -97,8 +97,9 @@ class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, const std::shared_ptr response); public: - explicit ContinentalRadarEthernetHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - ~ContinentalRadarEthernetHwInterfaceRosWrapper() noexcept override; + explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; + /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; @@ -111,13 +112,15 @@ class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration /// @return Resulting status - Status GetParameters(drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration); + Status GetParameters(drivers::ContinentalARS548SensorConfiguration & sensor_configuration); private: std::mutex mtx_config_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; diagnostic_updater::Updater diagnostics_updater_; + + /// @brief Callback to populate diagnostic messages void ContinentalMonitorStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief rclcpp parameter callback @@ -125,6 +128,7 @@ class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, /// @return SetParametersResult rcl_interfaces::msg::SetParametersResult paramCallback( const std::vector & parameters); + /// @brief Updating rclcpp parameter /// @return SetParametersResult std::vector updateParameters(); @@ -133,4 +137,4 @@ class ContinentalRadarEthernetHwInterfaceRosWrapper final : public rclcpp::Node, } // namespace ros } // namespace nebula -#endif // NEBULA_ContinentalRadarEthernetHwInterfaceRosWrapper_H +#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index a0a394bc4..dc2b1c51a 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto ros_environment + continental_msgs diagnostic_msgs diagnostic_updater libpcl-all-dev diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp new file mode 100644 index 000000000..a72982153 --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -0,0 +1,344 @@ +// Copyright 2023 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_ars548_decoder_ros_wrapper.hpp" + +#include "pcl_conversions/pcl_conversions.h" + +namespace nebula +{ +namespace ros +{ +ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_driver_ros_wrapper", options), hw_interface_() +{ + drivers::ContinentalARS548SensorConfiguration sensor_configuration; + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + + 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); + + wrapper_status_ = + InitializeDriver(std::const_pointer_cast(sensor_cfg_ptr_)); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); + packets_sub_ = create_subscription( + "nebula_packets", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback, this, std::placeholders::_1)); + + detection_list_pub_ = + this->create_publisher( + "detection_array", rclcpp::SensorDataQoS()); + object_list_pub_ = this->create_publisher( + "object_array", rclcpp::SensorDataQoS()); + + detection_pointcloud_pub_ = this->create_publisher( + "detection_points", rclcpp::SensorDataQoS()); + object_pointcloud_pub_ = + this->create_publisher("object_points", rclcpp::SensorDataQoS()); +} + +void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( + const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) +{ + decoder_ptr_->ProcessPackets(*scan_msg); +} + +Status ContinentalARS548DriverRosWrapper::InitializeDriver( + std::shared_ptr sensor_configuration) +{ + decoder_ptr_ = std::make_shared( + std::static_pointer_cast(sensor_configuration)); + + decoder_ptr_->RegisterDetectionListCallback(std::bind( + &ContinentalARS548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalARS548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); + + return Status::OK; +} + +Status ContinentalARS548DriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalARS548DriverRosWrapper::GetParameters( + drivers::ContinentalARS548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", "255.255.255.255", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", 2368, descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", ""); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "continental_ars548", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + std::shared_ptr sensor_cfg_ptr = + std::make_shared(sensor_configuration); + + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalARS548DriverRosWrapper::DetectionListCallback( + std::unique_ptr msg) +{ + if ( + detection_pointcloud_pub_->get_subscription_count() > 0 || + 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; + detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); + } + if ( + detection_list_pub_->get_subscription_count() > 0 || + detection_list_pub_->get_intra_process_subscription_count() > 0) { + detection_list_pub_->publish(std::move(msg)); + } +} + +void ContinentalARS548DriverRosWrapper::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 ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548DetectionList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + pcl::PointXYZ point{}; + for (const auto & detection : msg.detections) { + point.x = + std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; + point.y = + std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; + point.z = std::sin(detection.elevation_angle) * detection.range; + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + pcl::PointXYZ point{}; + for (const auto & detection : msg.objects) { + point.x = static_cast(detection.position.x); + point.y = static_cast(detection.position.y); + point.z = static_cast(detection.position.z); + + output_pointcloud->points.emplace_back(point); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ConvertToRadarScan( + const continental_msgs::msg::ContinentalArs548DetectionList & 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.invalid_azimuth || detection.invalid_distance || detection.invalid_elevation || + detection.invalid_range_rate) { + continue; + } + + return_msg.range = detection.range; + return_msg.azimuth = detection.azimuth_angle; + return_msg.elevation = detection.elevation_angle; + 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 ConvertToRadarTracks( + const continental_msgs::msg::ContinentalArs548ObjectList & 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 int16_t CAR_ID = 32001; + constexpr int16_t TRUCK_ID = 32002; + constexpr int16_t MOTORCYCLE_ID = 32005; + constexpr int16_t BICYCLE_ID = 32006; + constexpr int16_t PEDESTRIAN_ID = 32007; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & detection : msg.objects) { + track_msg.uuid.uuid[0] = static_cast(detection.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((detection.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((detection.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((detection.object_id >> 24) & 0xff); + track_msg.position = detection.position; + track_msg.velocity = detection.absolute_velocity; + track_msg.acceleration = detection.absolute_acceleration; + track_msg.size.x = detection.shape_length_edge_mean; + track_msg.size.y = detection.shape_width_edge_mean; + track_msg.size.z = 1.f; + + uint8_t max_score = detection.classification_unknown; + track_msg.classification = UNKNOWN_ID; + + if (detection.classification_car > max_score) { + max_score = detection.classification_car; + track_msg.classification = CAR_ID; + } + if (detection.classification_truck > max_score) { + max_score = detection.classification_truck; + track_msg.classification = TRUCK_ID; + } + if (detection.classification_motorcycle > max_score) { + max_score = detection.classification_motorcycle; + track_msg.classification = MOTORCYCLE_ID; + } + if (detection.classification_bicycle > max_score) { + max_score = detection.classification_bicycle; + track_msg.classification = BICYCLE_ID; + } + if (detection.classification_pedestrian > max_score) { + max_score = detection.classification_pedestrian; + track_msg.classification = PEDESTRIAN_ID; + } + + track_msg.position_covariance[0] = static_cast(detection.position_std.x); + track_msg.position_covariance[1] = detection.position_covariance_xy; + track_msg.position_covariance[2] = 0.f; + track_msg.position_covariance[3] = static_cast(detection.position_std.y); + track_msg.position_covariance[4] = 0.f; + track_msg.position_covariance[5] = static_cast(detection.position_std.z); + + track_msg.velocity_covariance[0] = static_cast(detection.absolute_velocity_std.x); + track_msg.velocity_covariance[1] = detection.absolute_velocity_covariance_xy; + track_msg.velocity_covariance[2] = 0.f; + track_msg.velocity_covariance[3] = static_cast(detection.absolute_velocity_std.y); + track_msg.velocity_covariance[4] = 0.f; + track_msg.velocity_covariance[5] = static_cast(detection.absolute_velocity_std.z); + + track_msg.acceleration_covariance[0] = + static_cast(detection.absolute_acceleration_std.x); + track_msg.acceleration_covariance[1] = detection.absolute_acceleration_covariance_xy; + track_msg.acceleration_covariance[2] = 0.f; + track_msg.acceleration_covariance[3] = + static_cast(detection.absolute_acceleration_std.y); + track_msg.acceleration_covariance[4] = 0.f; + track_msg.acceleration_covariance[5] = + static_cast(detection.absolute_acceleration_std.z); + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548DriverRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp similarity index 81% rename from nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp rename to nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 63352a6a3..1a2f4aa32 100644 --- a/nebula_ros/src/continental/continental_radar_ethernet_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_ros/continental/continental_radar_ethernet_hw_interface_ros_wrapper.hpp" +#include "nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp" #include #include @@ -21,7 +21,7 @@ namespace nebula { namespace ros { -ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalRadarEthernetHwInterfaceRosWrapper( +ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_hw_interface_ros_wrapper", options), hw_interface_(), @@ -37,67 +37,61 @@ ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalRadarEthernetHwInterfa } hw_interface_.SetLogger(std::make_shared(this->get_logger())); std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); + std::make_shared(sensor_configuration_); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); hw_interface_.RegisterScanCallback(std::bind( - &ContinentalRadarEthernetHwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + &ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, std::placeholders::_1)); packets_pub_ = this->create_publisher( "nebula_packets", rclcpp::SensorDataQoS()); -#if not defined(TEST_PCAP) // KL check how to do this - if (this->setup_sensor) { - set_param_res_ = this->add_on_set_parameters_callback(std::bind( - &ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - } -#endif + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); driving_direction_sub_ = create_subscription( "driving_direction", rclcpp::SensorDataQoS(), std::bind( - &ContinentalRadarEthernetHwInterfaceRosWrapper::DrivingDirectionCallback, this, + &ContinentalARS548HwInterfaceRosWrapper::DrivingDirectionCallback, this, std::placeholders::_1)); sensor_config_service_server_ = this->create_service( - "configure_radar", - std::bind( - &ContinentalRadarEthernetHwInterfaceRosWrapper::SensorConfigureRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); + "configure_radar", std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SensorConfigureRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); StreamStart(); } -ContinentalRadarEthernetHwInterfaceRosWrapper::~ContinentalRadarEthernetHwInterfaceRosWrapper() +ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper() { } -Status ContinentalRadarEthernetHwInterfaceRosWrapper::StreamStart() +Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() { using std::chrono_literals::operator""ms; if (Status::OK == interface_status_) { interface_status_ = hw_interface_.CloudInterfaceStart(); diagnostics_updater_.add( - "radar_status", this, - &ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalMonitorStatus); + "radar_status", this, &ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus); } return interface_status_; } -Status ContinentalRadarEthernetHwInterfaceRosWrapper::StreamStop() +Status ContinentalARS548HwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status ContinentalRadarEthernetHwInterfaceRosWrapper::Shutdown() +Status ContinentalARS548HwInterfaceRosWrapper::Shutdown() { return Status::OK; } -Status ContinentalRadarEthernetHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think - // this is needed +Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed const drivers::SensorConfigurationBase & sensor_configuration) { std::stringstream ss; @@ -106,8 +100,8 @@ Status ContinentalRadarEthernetHwInterfaceRosWrapper::InitializeHwInterface( // return Status::OK; } -Status ContinentalRadarEthernetHwInterfaceRosWrapper::GetParameters( - drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration) +Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( + drivers::ContinentalARS548SensorConfiguration & sensor_configuration) { { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -197,14 +191,13 @@ Status ContinentalRadarEthernetHwInterfaceRosWrapper::GetParameters( return Status::OK; } -void ContinentalRadarEthernetHwInterfaceRosWrapper::ReceivePacketsDataCallback( +void ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( std::unique_ptr scan_buffer) { packets_pub_->publish(std::move(scan_buffer)); } -rcl_interfaces::msg::SetParametersResult -ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback( +rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper::paramCallback( const std::vector & p) { std::scoped_lock lock(mtx_config_); @@ -213,7 +206,7 @@ ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback( RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); RCLCPP_INFO_STREAM(this->get_logger(), p); - drivers::ContinentalRadarEthernetSensorConfiguration new_param{sensor_configuration_}; + drivers::ContinentalARS548SensorConfiguration new_param{sensor_configuration_}; RCLCPP_INFO_STREAM(this->get_logger(), new_param); std::string sensor_model_str; std::string return_mode_str; @@ -231,7 +224,7 @@ ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback( sensor_configuration_ = new_param; RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); + std::make_shared(sensor_configuration_); RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); @@ -247,13 +240,9 @@ ContinentalRadarEthernetHwInterfaceRosWrapper::paramCallback( return *result; } -void ContinentalRadarEthernetHwInterfaceRosWrapper::DrivingDirectionCallback( +void ContinentalARS548HwInterfaceRosWrapper::DrivingDirectionCallback( [[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) { - std::cout << std::this_thread::get_id() - << ": ContinentalRadarEthernetHwInterface::DrivingDirectionTimerCallback" << std::endl - << std::flush; - hw_interface_.SetDrivingDirection(0); hw_interface_.SetAccelerationLateralCog(0.0); hw_interface_.SetAccelerationLongitudinalCog(0.0); @@ -264,17 +253,15 @@ void ContinentalRadarEthernetHwInterfaceRosWrapper::DrivingDirectionCallback( hw_interface_.SetYawRate(0.25); } -void ContinentalRadarEthernetHwInterfaceRosWrapper::SensorConfigureRequestCallback( +void ContinentalARS548HwInterfaceRosWrapper::SensorConfigureRequestCallback( [[maybe_unused]] const std::shared_ptr request, [[maybe_unused]] const std::shared_ptr response) { - std::cout << "SetVehicleParametersCallback" << std::endl << std::flush; - hw_interface_.SetVehicleParameters(10.f, 4.5f, 3.f, 2.5f); } std::vector -ContinentalRadarEthernetHwInterfaceRosWrapper::updateParameters() +ContinentalARS548HwInterfaceRosWrapper::updateParameters() { std::scoped_lock lock(mtx_config_); RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); @@ -294,14 +281,10 @@ ContinentalRadarEthernetHwInterfaceRosWrapper::updateParameters() return results; } -void ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalMonitorStatus( +void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - std::cout << std::this_thread::get_id() << ": ContinentalMonitorStatus" << std::endl - << std::flush; - auto sensor_status = hw_interface_.GetRadarStatus(); - // KL need to use smart pointers for the initial case diagnostics.add("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); diagnostics.add("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); diagnostics.add("timestamp_sync_status", sensor_status.timestamp_sync_status); @@ -339,11 +322,8 @@ void ContinentalRadarEthernetHwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostics.add("voltage_status", sensor_status.voltage_status); diagnostics.add("temperature_status", sensor_status.temperature_status); diagnostics.add("blockage_status", sensor_status.blockage_status); - diagnostics.summary( - diagnostic_msgs::msg::DiagnosticStatus::WARN, - "Dummy No data available"); // KL: need to check this } -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalRadarEthernetHwInterfaceRosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp deleted file mode 100644 index 947314c38..000000000 --- a/nebula_ros/src/continental/continental_radar_ethernet_decoder_ros_wrapper.cpp +++ /dev/null @@ -1,210 +0,0 @@ -// Copyright 2023 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_radar_ethernet_decoder_ros_wrapper.hpp" - -#include "pcl_conversions/pcl_conversions.h" - -namespace nebula -{ -namespace ros -{ -ContinentalRadarEthernetDriverRosWrapper::ContinentalRadarEthernetDriverRosWrapper( - const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_driver_ros_wrapper", options), hw_interface_() -{ - drivers::ContinentalRadarEthernetSensorConfiguration sensor_configuration; - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - hw_interface_.SetLogger(std::make_shared(this->get_logger())); - - 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); - - wrapper_status_ = - InitializeDriver(std::const_pointer_cast(sensor_cfg_ptr_)); - - RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); - packets_sub_ = create_subscription( - "nebula_packets", rclcpp::SensorDataQoS(), - std::bind( - &ContinentalRadarEthernetDriverRosWrapper::ReceivePacketsMsgCallback, this, - std::placeholders::_1)); - - detection_list_pub_ = - this->create_publisher( - "detection_array", rclcpp::SensorDataQoS()); - object_list_pub_ = this->create_publisher( - "object_array", rclcpp::SensorDataQoS()); - - detection_pointcloud_pub_ = this->create_publisher( - "detection_points", rclcpp::SensorDataQoS()); - object_pointcloud_pub_ = - this->create_publisher("object_points", rclcpp::SensorDataQoS()); -} - -void ContinentalRadarEthernetDriverRosWrapper::ReceivePacketsMsgCallback( - const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg) -{ - decoder_ptr_->ProcessPackets(*scan_msg); -} - -void ContinentalRadarEthernetDriverRosWrapper::PublishCloud( - std::unique_ptr pointcloud, - const rclcpp::Publisher::SharedPtr & publisher) -{ - if (pointcloud->header.stamp.sec < 0) { - RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source."); - } - pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id; - publisher->publish(std::move(pointcloud)); -} - -Status ContinentalRadarEthernetDriverRosWrapper::InitializeDriver( - std::shared_ptr sensor_configuration) -{ - decoder_ptr_ = std::make_shared( - std::static_pointer_cast( - sensor_configuration)); - - decoder_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalRadarEthernetDriverRosWrapper::detectionListCallback, this, std::placeholders::_1)); - decoder_ptr_->RegisterObjectListCallback(std::bind( - &ContinentalRadarEthernetDriverRosWrapper::objectListCallback, this, std::placeholders::_1)); - - return Status::OK; -} - -Status ContinentalRadarEthernetDriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} - -Status ContinentalRadarEthernetDriverRosWrapper::GetParameters( - drivers::ContinentalRadarEthernetSensorConfiguration & sensor_configuration) -{ - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); - sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); - sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); - sensor_configuration.data_port = this->get_parameter("data_port").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = false; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "continental_ars548", descriptor); - sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); - } - - if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { - return Status::INVALID_SENSOR_MODEL; - } - - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); - - hw_interface_.SetSensorConfiguration( - std::static_pointer_cast(sensor_cfg_ptr)); - - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); - return Status::OK; -} - -void ContinentalRadarEthernetDriverRosWrapper::detectionListCallback( - std::unique_ptr msg) -{ - if ( - detection_pointcloud_pub_->get_subscription_count() > 0 || - detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = - nebula::drivers::continental_ars548::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; - detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); - } - if ( - detection_list_pub_->get_subscription_count() > 0 || - detection_list_pub_->get_intra_process_subscription_count() > 0) { - detection_list_pub_->publish(std::move(msg)); - } -} - -void ContinentalRadarEthernetDriverRosWrapper::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 = - nebula::drivers::continental_ars548::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 ( - object_list_pub_->get_subscription_count() > 0 || - object_list_pub_->get_intra_process_subscription_count() > 0) { - object_list_pub_->publish(std::move(msg)); - } -} - -RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalRadarEthernetDriverRosWrapper) -} // namespace ros -} // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp index 5d562e096..ffaf10e62 100644 --- a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp @@ -52,7 +52,7 @@ Status RobosenseHwMonitorRosWrapper::Shutdown() { return Status::OK; } -// + Status RobosenseHwMonitorRosWrapper::InitializeHwMonitor( const drivers::SensorConfigurationBase & sensor_configuration) { From ad02d9a06a140e678e25013df6e2961f0be6e1ad Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 13 Dec 2023 20:16:04 +0900 Subject: [PATCH 03/42] fix: fixed errors with naming and symbols after the refactoring Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/CMakeLists.txt | 16 ++++++++-------- nebula_ros/launch/continental_launch_all_hw.xml | 8 ++++---- .../continental_ars548_decoder_ros_wrapper.cpp | 6 +++--- ...ntinental_ars548_hw_interface_ros_wrapper.cpp | 2 +- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index fea064177..31ff306e8 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -125,21 +125,21 @@ rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper ## Continental # Hw Interface -ament_auto_add_library(continental_hw_ros_wrapper SHARED +ament_auto_add_library(continental_ars548_hw_ros_wrapper SHARED src/continental/continental_ars548_hw_interface_ros_wrapper.cpp ) -rclcpp_components_register_node(continental_hw_ros_wrapper - PLUGIN "ContinentalRadarEthernetHwInterfaceRosWrapper" - EXECUTABLE continental_hw_interface_ros_wrapper_node +rclcpp_components_register_node(continental_ars548_hw_ros_wrapper + PLUGIN "ContinentalARS548HwInterfaceRosWrapper" + EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node ) # DriverDecoder -ament_auto_add_library(continental_driver_ros_wrapper SHARED +ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED src/continental/continental_ars548_decoder_ros_wrapper.cpp ) -rclcpp_components_register_node(continental_driver_ros_wrapper - PLUGIN "ContinentalRadarEthernetDriverRosWrapper" - EXECUTABLE continental_driver_ros_wrapper_node +rclcpp_components_register_node(continental_ars548_driver_ros_wrapper + PLUGIN "ContinentalARS548DriverRosWrapper" + EXECUTABLE continental_ars548_driver_ros_wrapper_node ) diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 03e01aee0..5ee173884 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -1,6 +1,6 @@ - + @@ -12,7 +12,7 @@ - + @@ -20,8 +20,8 @@ - + diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index a72982153..3f6879938 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -22,7 +22,7 @@ namespace ros { ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_driver_ros_wrapper", options), hw_interface_() +: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() { drivers::ContinentalARS548SensorConfiguration sensor_configuration; @@ -190,7 +190,7 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } -pcl::PointCloud::Ptr ConvertToPointcloud( +pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); @@ -212,7 +212,7 @@ pcl::PointCloud::Ptr ConvertToPointcloud( return output_pointcloud; } -pcl::PointCloud::Ptr ConvertToPointcloud( +pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 1a2f4aa32..ef3cc8ebc 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -23,7 +23,7 @@ namespace ros { ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( const rclcpp::NodeOptions & options) -: rclcpp::Node("hesai_hw_interface_ros_wrapper", options), +: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_(), diagnostics_updater_(this) { From 3d071a250d3ab29c761bf64269aaf9aff3353354 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 13 Dec 2023 21:07:10 +0900 Subject: [PATCH 04/42] chore: fixed spell misses Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 5 +- .../continental/continental_ars548.hpp | 10 +-- .../continental/continental_common.hpp | 2 +- .../decoders/continental_ars548_decoder.cpp | 10 +-- .../continental_ars548_hw_interface.hpp | 6 +- .../continental_types.hpp | 6 +- .../continental_ars548_hw_interface.cpp | 80 +++++++++---------- .../msg/ContinentalArs548Detection.msg | 2 +- ...nental_ars548_hw_interface_ros_wrapper.cpp | 2 +- 9 files changed, 63 insertions(+), 60 deletions(-) diff --git a/.cspell.json b/.cspell.json index e2b0768d9..f94b55d8c 100644 --- a/.cspell.json +++ b/.cspell.json @@ -38,6 +38,9 @@ "Difop", "gptp", "Idat", - "Vdat" + "Vdat", + "autosar", + "characteristic", + "srvs" ] } diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index fd68e721a..a80e7e811 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -59,7 +59,7 @@ constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; constexpr int STATUS_CYCLE_TIME_BYTE = 60; constexpr int STATUS_TIME_SLOT_BYTE = 61; constexpr int STATUS_HCC_BYTE = 62; -constexpr int STATUS_POWERSAVING_STANDSTILL_BYTE = 63; +constexpr int STATUS_POWER_SAVING_STANDSTILL_BYTE = 63; constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; @@ -80,10 +80,10 @@ constexpr int OBJECT_LIST_METHOD_ID = 329; constexpr int SENSOR_STATUS_METHOD_ID = 380; constexpr int FILTER_STATUS_METHOD_ID = 396; -constexpr int DETECTION_LIST_UDP_PAYPLOAD = 35336; -constexpr int OBJECT_LIST_UDP_PAYPLOAD = 9401; -constexpr int SENSOR_STATUS_UDP_PAYPLOAD = 84; -constexpr int FILTER_STATUS_UDP_PAYPLOAD = 330; +constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; +constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; +constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; +constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; constexpr int DETECTION_LIST_PDU_LENGTH = 35328; constexpr int OBJECT_LIST_PDU_LENGTH = 9393; diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp index 439cb2ead..078f7fe0d 100644 --- a/nebula_common/include/nebula_common/continental/continental_common.hpp +++ b/nebula_common/include/nebula_common/continental/continental_common.hpp @@ -29,7 +29,7 @@ namespace nebula { namespace drivers { -/// @brief struct for Hesai sensor configuration +/// @brief struct for ARS548 sensor configuration struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; 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 99b6a7ba7..8fc0ef81f 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 @@ -75,13 +75,13 @@ bool ContinentalARS548Decoder::ProcessPackets( } if (method_id == DETECTION_LIST_METHOD_ID) { - if (data.size() != DETECTION_LIST_UDP_PAYPLOAD || length != DETECTION_LIST_PDU_LENGTH) { + if (data.size() != DETECTION_LIST_UDP_PAYLOAD || length != DETECTION_LIST_PDU_LENGTH) { return false; } return ParseDetectionsListPacket(data); } else if (method_id == OBJECT_LIST_METHOD_ID) { - if (data.size() != OBJECT_LIST_UDP_PAYPLOAD || length != OBJECT_LIST_PDU_LENGTH) { + if (data.size() != OBJECT_LIST_UDP_PAYLOAD || length != OBJECT_LIST_PDU_LENGTH) { return false; } @@ -305,7 +305,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(data[CURRENT_DETECTION_OBJECT_ID_BYTE]) << 8) | static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE + 1]); @@ -313,7 +313,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector 1514 || frequency_slot > 2 || cycle_time < 50 || cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 || - powersave_standstill > 1) { + power_save_standstill > 1) { PrintError("Invalid SetRadarParameters values"); return Status::SENSOR_CONFIG_ERROR; } @@ -682,7 +682,7 @@ Status ContinentalARS548HwInterface::SetRadarParameters( send_vector[CONFIGURATION_CYCLE_TIME_BYTE] = cycle_time; send_vector[CONFIGURATION_TIME_SLOT_BYTE] = time_slot; send_vector[CONFIGURATION_HCC_BYTE] = hcc; - send_vector[CONFIGURATION_POWERSAVE_STANDSTILL_BYTE] = powersave_standstill; + send_vector[CONFIGURATION_power_save_STANDSTILL_BYTE] = power_save_standstill; send_vector[CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE] = 1; @@ -878,19 +878,19 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) { - constexpr uint16_t VELOCITY_VECHILE_SERVICE_ID = 0; - constexpr uint16_t VELOCITY_VECHILE_METHOD_ID = 323; - constexpr uint8_t VELOCITY_VECHILE_LENGTH = 28; - const int VELOCITY_VECHILE_PAYLOAD_SIZE = VELOCITY_VECHILE_LENGTH + 8; + constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; + constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; + constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; + const int VELOCITY_VEHICLE_PAYLOAD_SIZE = VELOCITY_VEHICLE_LENGTH + 8; uint8_t bytes[4]; std::memcpy(bytes, &velocity, sizeof(velocity)); - std::vector send_vector(VELOCITY_VECHILE_PAYLOAD_SIZE, 0); - send_vector[1] = VELOCITY_VECHILE_SERVICE_ID; - send_vector[2] = static_cast(VELOCITY_VECHILE_METHOD_ID >> 8); - send_vector[3] = static_cast(VELOCITY_VECHILE_METHOD_ID & 0x00ff); - send_vector[7] = VELOCITY_VECHILE_LENGTH; + std::vector send_vector(VELOCITY_VEHICLE_PAYLOAD_SIZE, 0); + send_vector[1] = VELOCITY_VEHICLE_SERVICE_ID; + send_vector[2] = static_cast(VELOCITY_VEHICLE_METHOD_ID >> 8); + send_vector[3] = static_cast(VELOCITY_VEHICLE_METHOD_ID & 0x00ff); + send_vector[7] = VELOCITY_VEHICLE_LENGTH; send_vector[11] = bytes[3]; send_vector[12] = bytes[2]; send_vector[13] = bytes[1]; diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg index a2505ae11..f6d7e40de 100644 --- a/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548Detection.msg @@ -18,6 +18,6 @@ int8 rcs uint16 measurement_id uint8 positive_predictive_value uint8 classification -uint8 multi_target_probabilty +uint8 multi_target_probability uint16 object_id uint8 ambiguity_flag diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index ef3cc8ebc..60e4e98af 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -306,7 +306,7 @@ void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostics.add("cycle_time", std::to_string(sensor_status.cycle_time)); diagnostics.add("time_slot", std::to_string(sensor_status.time_slot)); diagnostics.add("hcc", sensor_status.hcc); - diagnostics.add("powersave_standstill", sensor_status.powersave_standstill); + diagnostics.add("power_save_standstill", sensor_status.power_save_standstill); diagnostics.add("sensor_ip_address0", sensor_status.sensor_ip_address0); diagnostics.add("sensor_ip_address1", sensor_status.sensor_ip_address1); diagnostics.add("configuration_counter", std::to_string(sensor_status.configuration_counter)); From c08b660dd9945a95cda0462db8f5c49a2d7cd2ed Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 13 Dec 2023 21:13:03 +0900 Subject: [PATCH 05/42] chore: more spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 - .../continental/continental_ars548.hpp | 2 +- .../continental_ars548_hw_interface.hpp | 6 ++-- .../continental_types.hpp | 4 +-- .../continental_ars548_hw_interface.cpp | 32 +++++++++---------- ...nental_ars548_hw_interface_ros_wrapper.cpp | 4 +-- 6 files changed, 24 insertions(+), 25 deletions(-) diff --git a/.cspell.json b/.cspell.json index f94b55d8c..4cd9038ca 100644 --- a/.cspell.json +++ b/.cspell.json @@ -40,7 +40,6 @@ "Idat", "Vdat", "autosar", - "characteristic", "srvs" ] } diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index a80e7e811..95f4bc001 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -69,7 +69,7 @@ constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; constexpr int STATUS_YAW_RATE_BYTE = 76; constexpr int STATUS_STEERING_ANGLE_BYTE = 77; constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; -constexpr int STATUS_CHARASTERISTIC_SPEED_BYTE = 79; +constexpr int STATUS_CHARACTERISTIC_SPEED_BYTE = 79; constexpr int STATUS_RADAR_STATUS_BYTE = 80; constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; 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 1a6223f6f..da2f12cc4 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 @@ -198,10 +198,10 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @return Resulting status Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); - /// @brief Set the charasteristic speed - /// @param charasteristic_speed Charasteristic speed + /// @brief Set the characteristic speed + /// @param characteristic_speed Characteristic speed /// @return Resulting status - Status SetCharasteristicSpeed(float charasteristic_speed); + Status SetCharacteristicSpeed(float characteristic_speed); /// @brief Set the current direction /// @param direction Current driving direction diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index c567355b6..407632844 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -58,7 +58,7 @@ struct ContinentalARS548Status std::string status_yaw_rate; std::string status_steering_angle; std::string status_driving_direction; - std::string charasteristic_speed; + std::string characteristic_speed; std::string radar_status; std::string voltage_status; std::string temperature_status; @@ -134,7 +134,7 @@ struct ContinentalARS548Status os << ", "; os << "status_driving_direction: " << arg.status_driving_direction; os << ", "; - os << "charasteristic_speed: " << arg.charasteristic_speed; + os << "characteristic_speed: " << arg.characteristic_speed; os << ", "; os << "radar_status: " << arg.radar_status; os << ", "; 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 ba19dc74d..44ce05a95 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 @@ -328,9 +328,9 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector 255.f) { - PrintError("Invalid charasteristic_speed value"); + if (characteristic_speed < 0.f || characteristic_speed > 255.f) { + PrintError("Invalid characteristic_speed value"); return Status::ERROR_1; } - std::vector send_vector(CHARASTERISTIC_SPEED_PAYLOAD_SIZE, 0); - send_vector[1] = CHARASTERISTIC_SPEED_SERVICE_ID; - send_vector[2] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID >> 8); - send_vector[3] = static_cast(CHARASTERISTIC_SPEED_METHOD_ID & 0x00ff); - send_vector[7] = CHARASTERISTIC_SPEED_LENGTH; - send_vector[10] = static_cast(charasteristic_speed); + std::vector send_vector(CHARACTERISTIC_SPEED_PAYLOAD_SIZE, 0); + send_vector[1] = CHARACTERISTIC_SPEED_SERVICE_ID; + send_vector[2] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID >> 8); + send_vector[3] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID & 0x00ff); + send_vector[7] = CHARACTERISTIC_SPEED_LENGTH; + send_vector[10] = static_cast(characteristic_speed); sensor_udp_driver_->sender()->asyncSend(send_vector); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 60e4e98af..927a8ce44 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -247,7 +247,7 @@ void ContinentalARS548HwInterfaceRosWrapper::DrivingDirectionCallback( hw_interface_.SetAccelerationLateralCog(0.0); hw_interface_.SetAccelerationLongitudinalCog(0.0); - hw_interface_.SetCharasteristicSpeed(0.0); + hw_interface_.SetCharacteristicSpeed(0.0); hw_interface_.SetSteeringAngleFrontAxle(0.0); hw_interface_.SetVelocityVehicle(5.0); hw_interface_.SetYawRate(0.25); @@ -317,7 +317,7 @@ void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostics.add("status_yaw_rate", sensor_status.status_yaw_rate); diagnostics.add("status_steering_angle", sensor_status.status_steering_angle); diagnostics.add("status_driving_direction", sensor_status.status_driving_direction); - diagnostics.add("charasteristic_speed", sensor_status.charasteristic_speed); + diagnostics.add("characteristic_speed", sensor_status.characteristic_speed); diagnostics.add("radar_status", sensor_status.radar_status); diagnostics.add("voltage_status", sensor_status.voltage_status); diagnostics.add("temperature_status", sensor_status.temperature_status); From b1976647b257b65c01c50a8dc9c4706cb9c3e9c3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 14:47:43 +0900 Subject: [PATCH 06/42] fix: updated the build_repos to point to the custom transport drivers for this PR Signed-off-by: Kenzo Lobos-Tsunekawa --- build_depends.repos | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index e28379f1a..6530d0b5d 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -2,5 +2,5 @@ repositories: # TCP version of transport drivers transport_drivers: type: git - url: https://github.com/MapIV/transport_drivers - version: boost + url: https://github.com/knzo25/transport_drivers + version: feat/multicast_support From b7f36e35ac16c32d8dd5b9f5d28bc0f675759869 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 14:48:59 +0900 Subject: [PATCH 07/42] chore: removed duplicated message file Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_messages/continental_msgs/msg/NebulaPackets.msg | 2 -- 1 file changed, 2 deletions(-) delete mode 100644 nebula_messages/continental_msgs/msg/NebulaPackets.msg diff --git a/nebula_messages/continental_msgs/msg/NebulaPackets.msg b/nebula_messages/continental_msgs/msg/NebulaPackets.msg deleted file mode 100644 index 7aa8444f1..000000000 --- a/nebula_messages/continental_msgs/msg/NebulaPackets.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -NebulaPacket[] packets From b672eb52dbb461b76214fe483689413ab3842cdf Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 14 Dec 2023 16:57:40 +0900 Subject: [PATCH 08/42] Update nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- .../continental/continental_ars548_hw_interface_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 927a8ce44..27f6931ef 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -36,7 +36,7 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( return; } hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr sensor_cfg_ptr = + std::shared_ptr sensor_cfg_ptr = std::make_shared(sensor_configuration_); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); From ee0802cde606d29fc231dfe0a5c03fc9f4171e07 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 17:01:38 +0900 Subject: [PATCH 09/42] chore: addressed comments from the review related con consitency and unused code Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_hw_interface.hpp | 3 --- nebula_ros/launch/continental_launch_all_hw.xml | 4 +--- .../continental/continental_ars548_decoder_ros_wrapper.cpp | 2 +- 3 files changed, 2 insertions(+), 7 deletions(-) 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 da2f12cc4..94424b5d7 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 @@ -99,9 +99,6 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase public: /// @brief Constructor ContinentalARS548HwInterface(); - /// @brief Initializing tcp_driver for TCP communication - /// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration - /// @return Resulting status /// @brief Process a new sensor status packet /// @param buffer The buffer containing the status packet diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 5ee173884..6d487803b 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -1,7 +1,7 @@ - + @@ -10,8 +10,6 @@ - - diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 3f6879938..02544d61c 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -132,7 +132,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "continental_ars548", descriptor); + this->declare_parameter("frame_id", "continental", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } From 31703e5ab2c252069ac509627a2d002f7ba1dd35 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 19:36:36 +0900 Subject: [PATCH 10/42] chore: removed default parameters in line with the awf guidelines Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_decoder_ros_wrapper.cpp | 10 +++++----- ...ntinental_ars548_hw_interface_ros_wrapper.cpp | 16 ++++++++-------- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 02544d61c..caa0c28dc 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -95,7 +95,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); + this->declare_parameter("host_ip", descriptor); sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); } { @@ -104,7 +104,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); + this->declare_parameter("sensor_ip", descriptor); sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); } { @@ -113,7 +113,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); + this->declare_parameter("data_port", descriptor); sensor_configuration.data_port = this->get_parameter("data_port").as_int(); } { @@ -122,7 +122,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); + this->declare_parameter("sensor_model"); sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); } @@ -132,7 +132,7 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "continental", descriptor); + this->declare_parameter("frame_id", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 27f6931ef..d7b49be85 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -109,7 +109,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("sensor_model", ""); + this->declare_parameter("sensor_model", descriptor); sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); } @@ -119,7 +119,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("host_ip", "255.255.255.255", descriptor); + this->declare_parameter("host_ip", descriptor); sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); } { @@ -128,7 +128,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("sensor_ip", "192.168.1.201", descriptor); + this->declare_parameter("sensor_ip", descriptor); sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); } { @@ -137,7 +137,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("multicast_ip", "224.0.0.1", descriptor); + this->declare_parameter("multicast_ip", descriptor); sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); } { @@ -146,7 +146,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("frame_id", "continental", descriptor); + this->declare_parameter("frame_id", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } { @@ -155,7 +155,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("data_port", 2368, descriptor); + this->declare_parameter("data_port", descriptor); sensor_configuration.data_port = this->get_parameter("data_port").as_int(); } { @@ -164,7 +164,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("configuration_host_port", 2368, descriptor); + this->declare_parameter("configuration_host_port", descriptor); sensor_configuration.configuration_host_port = this->get_parameter("configuration_host_port").as_int(); } @@ -174,7 +174,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("configuration_sensor_port", 2368, descriptor); + this->declare_parameter("configuration_sensor_port", descriptor); sensor_configuration.configuration_sensor_port = this->get_parameter("configuration_sensor_port").as_int(); } From d383931fafc976457b3ba46b577ababd5aec2c20 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 19:46:37 +0900 Subject: [PATCH 11/42] chore: replaced the confusing position orientation name by simply otientation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 4 +- .../decoders/continental_ars548_decoder.cpp | 44 +++++++++---------- .../msg/ContinentalArs548Object.msg | 4 +- 3 files changed, 25 insertions(+), 27 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 95f4bc001..3a03b6004 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -155,8 +155,8 @@ constexpr int OBJECT_POSITION_Z_BYTE = 29; constexpr int OBJECT_POSITION_Z_STD_BYTE = 33; constexpr int OBJECT_POSITION_COVARIANCE_XY_BYTE = 37; -constexpr int OBJECT_POSITION_ORIENTATION_BYTE = 41; -constexpr int OBJECT_POSITION_ORIENTATION_STD_BYTE = 45; +constexpr int OBJECT_ORIENTATION_BYTE = 41; +constexpr int OBJECT_ORIENTATION_STD_BYTE = 45; constexpr int OBJECT_EXISTENCE_PROBABILITY_BYTE = 50; 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 8fc0ef81f..5c2a086a7 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 @@ -405,10 +405,9 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector const int CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_COVARIANCE_XY_BYTE; - const int CURRENT_OBJECT_POSITION_ORIENTATION_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_ORIENTATION_BYTE; - const int CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_ORIENTATION_STD_BYTE; + const int CURRENT_OBJECT_ORIENTATION_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_BYTE; + const int CURRENT_OBJECT_ORIENTATION_STD_BYTE = + CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_STD_BYTE; const int CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_EXISTENCE_PROBABILITY_BYTE; @@ -529,16 +528,16 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 1]) << 16) | (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 2]) << 8) | data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 3]; - const uint32_t position_orientation_u = - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_ORIENTATION_BYTE + 3]; - const uint32_t position_orientation_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_ORIENTATION_STD_BYTE + 3]; + const uint32_t orientation_u = + (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_ORIENTATION_BYTE + 3]; + const uint32_t orientation_std_u = + (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE]) << 24) | + (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 1]) << 16) | + (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 2]) << 8) | + data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 3]; const uint32_t existence_probability_u = (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE]) << 24) | (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 1]) << 16) | @@ -552,8 +551,8 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector float position_z_f; float position_z_std_f; float position_xy_covariance_f; - float position_orientation_f; - float position_orientation_std_f; + float orientation_f; + float orientation_std_f; float existence_probability_f; std::memcpy(&position_x_f, &position_x_u, sizeof(position_x_u)); @@ -564,9 +563,8 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector std::memcpy(&position_z_std_f, &position_z_std_u, sizeof(position_z_std_u)); std::memcpy( &position_xy_covariance_f, &position_xy_covariance_u, sizeof(position_xy_covariance_u)); - std::memcpy(&position_orientation_f, &position_orientation_u, sizeof(position_orientation_u)); - std::memcpy( - &position_orientation_std_f, &position_orientation_std_u, sizeof(position_orientation_std_u)); + std::memcpy(&orientation_f, &orientation_u, sizeof(orientation_u)); + std::memcpy(&orientation_std_f, &orientation_std_u, sizeof(orientation_std_u)); std::memcpy( &existence_probability_f, &existence_probability_u, sizeof(existence_probability_u)); @@ -799,8 +797,8 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector assert(position_y_std_f >= 0.f); assert(position_z_f >= -1600.f && position_z_f <= 1600.f); assert(position_z_std_f >= 0.f); - assert(position_orientation_f >= -M_PI && position_orientation_f <= M_PI); - assert(position_orientation_std_f >= 0.f); + assert(orientation_f >= -M_PI && orientation_f <= M_PI); + assert(orientation_std_f >= 0.f); assert(classification_car_u <= 100); assert(classification_truck_u <= 100); @@ -839,8 +837,8 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector object_msg.position_covariance_xy = position_xy_covariance_f; - object_msg.position_orientation = position_orientation_f; - object_msg.position_orientation_std = position_orientation_std_f; + object_msg.orientation = orientation_f; + object_msg.orientation_std = orientation_std_f; object_msg.existence_probability = existence_probability_f; object_msg.classification_car = classification_car_u; diff --git a/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg b/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg index 80fca0ad9..a10c1ad36 100644 --- a/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg +++ b/nebula_messages/continental_msgs/msg/ContinentalArs548Object.msg @@ -7,8 +7,8 @@ uint8 position_reference geometry_msgs/Point position geometry_msgs/Vector3 position_std float32 position_covariance_xy -float32 position_orientation -float32 position_orientation_std +float32 orientation +float32 orientation_std float32 existence_probability From 00966187cbf485f20fc546b8e0377804a362b356 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Dec 2023 19:51:18 +0900 Subject: [PATCH 12/42] chore: removed unused `using` Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548_hw_interface_ros_wrapper.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index d7b49be85..1234abd06 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -70,8 +70,6 @@ ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper( Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() { - using std::chrono_literals::operator""ms; - if (Status::OK == interface_status_) { interface_status_ = hw_interface_.CloudInterfaceStart(); diagnostics_updater_.add( From 2ee484f2a9540c5ddb5c12d64ba5e522cad42971 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 20 Dec 2023 13:56:16 +0900 Subject: [PATCH 13/42] feat: implemented boost endian approach Also fixed some typos Both implementations provide the same results Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 291 ++++++++++++++++++ .../decoders/continental_ars548_decoder.cpp | 254 ++++++++++++++- .../continental_ars548_hw_interface.cpp | 240 ++++++++++++++- 3 files changed, 756 insertions(+), 29 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 3a03b6004..e728a961e 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -16,6 +16,8 @@ /** * Continental ARS548 */ +#include "boost/endian/buffers.hpp" + #include #include @@ -29,10 +31,299 @@ namespace drivers { namespace continental_ars548 { + +using namespace boost::endian; + +#pragma pack(push, 1) + +struct Header +{ + big_uint16_buf_t service_id; + big_uint16_buf_t method_id; + big_uint32_buf_t length; +}; + +struct HeaderSOMEIP +{ + big_uint16_buf_t client_id; + big_uint16_buf_t session_id; + uint8_t protocol_version; + uint8_t interface_version; + uint8_t message_type; + uint8_t return_code; +}; + +struct HeaderE2EP07 +{ + big_uint64_buf_t crc; + big_uint32_buf_t length; + big_uint32_buf_t sqc; + big_uint32_buf_t data_id; +}; + +struct StampSyncStatus +{ + big_uint32_buf_t timestamp_nanoseconds; + big_uint32_buf_t timestamp_seconds; + uint8_t timestamp_sync_status; +}; + +struct Detection +{ // Actual 44 . Datasheet is 44 + big_float32_buf_t azimuth_angle; + big_float32_buf_t azimuth_angle_std; + uint8_t invalid_flags; + big_float32_buf_t elevation_angle; + big_float32_buf_t elevation_angle_std; + big_float32_buf_t range; + big_float32_buf_t range_std; + big_float32_buf_t range_rate; + big_float32_buf_t range_rate_std; + int8_t rcs; + big_uint16_buf_t measurement_id; + uint8_t positive_predictive_value; + uint8_t classification; + uint8_t multi_target_probability; + big_uint16_buf_t object_id; + uint8_t ambiguity_flag; + big_uint16_buf_t sort_index; +}; + +struct DetectionList +{ + Header header; + HeaderSOMEIP header_someip; + HeaderE2EP07 header_e2ep07; + StampSyncStatus stamp; + big_uint32_buf_t event_data_qualifier; + uint8_t extended_qualifier; + big_uint16_buf_t origin_invalid_flags; + big_float32_buf_t origin_x_pos; + big_float32_buf_t origin_x_std; + big_float32_buf_t origin_y_pos; + big_float32_buf_t origin_y_std; + big_float32_buf_t origin_z_pos; + big_float32_buf_t origin_z_std; + big_float32_buf_t origin_roll; + big_float32_buf_t origin_roll_std; + big_float32_buf_t origin_pitch; + big_float32_buf_t origin_pitch_std; + big_float32_buf_t origin_yaw; + big_float32_buf_t origin_yaw_std; + uint8_t list_invalid_flags; + Detection detections[800]; + big_float32_buf_t list_rad_vel_domain_min; + big_float32_buf_t list_rad_vel_domain_max; + big_uint32_buf_t number_of_detections; + big_float32_buf_t alignment_azimuth_correction; + big_float32_buf_t alignment_elevation_correction; + uint8_t alignment_status; + uint8_t reserverd[14]; +}; + +struct Object +{ // Datasheet 187 Current 184. Datsheet: 32x40 + 16x3 + 8x21 + big_uint16_buf_t status_sensor; // Current 32x40 16x3 21x1 + big_uint32_buf_t id; + big_uint16_buf_t age; + uint8_t status_measurement; + uint8_t status_movement; + big_uint16_buf_t position_invalid_flags; + uint8_t position_reference; + big_float32_buf_t position_x; + big_float32_buf_t position_x_std; + big_float32_buf_t position_y; + big_float32_buf_t position_y_std; + big_float32_buf_t position_z; + big_float32_buf_t position_z_std; + big_float32_buf_t position_covariance_xy; + big_float32_buf_t position_orientation; + big_float32_buf_t position_orientation_std; + uint8_t existance_invalid_flags; + big_float32_buf_t existance_probability; + big_float32_buf_t existance_ppv; + uint8_t classification_car; + uint8_t classification_truck; + uint8_t classification_motorcycle; + uint8_t classification_bicycle; + uint8_t classification_pedestrian; + uint8_t classification_animal; + uint8_t classification_hazard; + uint8_t classification_unknown; + uint8_t classification_overdrivable; + uint8_t classification_underdrivable; + uint8_t dynamics_abs_vel_invalid_flags; + big_float32_buf_t dynamics_abs_vel_x; + big_float32_buf_t dynamics_abs_vel_x_std; + big_float32_buf_t dynamics_abs_vel_y; + big_float32_buf_t dynamics_abs_vel_y_std; + big_float32_buf_t dynamics_abs_vel_covariance_xy; + uint8_t dynamics_rel_vel_invalid_flags; + big_float32_buf_t dynamics_rel_vel_x; + big_float32_buf_t dynamics_rel_vel_x_std; + big_float32_buf_t dynamics_rel_vel_y; + big_float32_buf_t dynamics_rel_vel_y_std; + big_float32_buf_t dynamics_rel_vel_covariance_xy; + uint8_t dynamics_abs_accel_invalid_flags; + big_float32_buf_t dynamics_abs_accel_x; + big_float32_buf_t dynamics_abs_accel_x_std; + big_float32_buf_t dynamics_abs_accel_y; + big_float32_buf_t dynamics_abs_accel_y_std; + big_float32_buf_t dynamics_abs_accel_covariance_xy; + uint8_t dynamics_rel_accel_invalid_flags; + big_float32_buf_t dynamics_rel_accel_x; + big_float32_buf_t dynamics_rel_accel_x_std; + big_float32_buf_t dynamics_rel_accel_y; + big_float32_buf_t dynamics_rel_accel_y_std; + big_float32_buf_t dynamics_rel_accel_covariance_xy; + uint8_t dynamics_orientation_invalid_flags; + big_float32_buf_t dynamics_orientation_rate_mean; + big_float32_buf_t dynamics_orientation_rate_std; + big_uint32_buf_t shape_length_status; + uint8_t shape_length_edge_invalid_flags; + big_float32_buf_t shape_length_edge_mean; + big_float32_buf_t shape_length_edge_std; + big_uint32_buf_t shape_width_status; + uint8_t shape_width_edge_invalid_flags; + big_float32_buf_t shape_width_edge_mean; + big_float32_buf_t shape_width_edge_std; +}; + +struct ObjectList +{ + Header header; + HeaderSOMEIP header_someip; + HeaderE2EP07 header_e2ep07; + StampSyncStatus stamp; + big_uint32_buf_t event_data_qualifier; + uint8_t extended_qualifier; + uint8_t number_of_objects; + Object objects[50]; +}; + +struct StatusConfiguration +{ + big_float32_buf_t longitudinal; + big_float32_buf_t lateral; + big_float32_buf_t vertical; + big_float32_buf_t yaw; + big_float32_buf_t pitch; + uint8_t plug_orientation; + big_float32_buf_t length; + big_float32_buf_t width; + big_float32_buf_t height; + big_float32_buf_t wheelbase; + big_uint16_buf_t maximum_distance; + uint8_t frequency_slot; + uint8_t cycle_time; + uint8_t time_slot; + uint8_t hcc; + uint8_t powersave_standstill; + uint8_t sensor_ip_address00; + uint8_t sensor_ip_address01; + uint8_t sensor_ip_address02; + uint8_t sensor_ip_address03; + uint8_t sensor_ip_address10; + uint8_t sensor_ip_address11; + uint8_t sensor_ip_address12; + uint8_t sensor_ip_address13; +}; + +struct SensorStatus +{ // Actual 44 + 2 + 30 = 76 bytes. datasheet: 76 + Header header; + StampSyncStatus stamp; + uint8_t sw_version_major; + uint8_t sw_version_minor; + uint8_t sw_version_patch; + StatusConfiguration status; + uint8_t configuration_counter; + uint8_t status_longitudinal_velocity; + uint8_t status_longitudinal_acceleration; + uint8_t status_lateral_acceleration; + uint8_t status_yaw_rate; + uint8_t status_steering_angle; + uint8_t status_driving_direction; + uint8_t status_characteristic_speed; + uint8_t status_radar_status; + uint8_t status_voltage_status; + uint8_t status_temperature_status; + uint8_t status_blockage_status; +}; + +struct Configuration +{ + Header header; + StatusConfiguration configuration; + uint8_t new_sensor_mounting; + uint8_t new_vehicle_parameters; + uint8_t new_radar_parameters; + uint8_t new_network_configuration; +}; + +struct AccelerationLateralCoG +{ + Header header; + uint8_t reserved0[6]; + big_float32_buf_t acceleration_lateral; + uint8_t reserved1[22]; +}; + +struct AccelerationLongitudinalCoG +{ + Header header; + uint8_t reserved0[6]; + big_float32_buf_t acceleration_lateral; + uint8_t reserved1[22]; +}; + +struct CharasteristicSpeed +{ + Header header; + uint8_t reserved0[2]; + big_float32_buf_t characteristic_speed; + uint8_t reserved1[8]; +}; + +struct DrivingDirection +{ + Header header; + uint8_t reserved0; + big_float32_buf_t driving_direction; + uint8_t reserved1[20]; +}; + +struct SteeringAngleFrontAxle +{ + Header header; + uint8_t reserved0[6]; + big_float32_buf_t steering_angle_front_axle; + uint8_t reserved1[22]; +}; + +struct VelocityVehicle +{ + Header header; + uint8_t reserved0[3]; + big_float32_buf_t velocity_vehicle; + uint8_t reserved1[21]; +}; + +struct YawRate +{ + Header header; + uint8_t reserved0[6]; + big_float32_buf_t yaw_rate; + uint8_t reserved1[221]; +}; + +#pragma pack(pop) + constexpr int SERVICE_ID_BYTE = 0; constexpr int METHOD_ID_BYTE = 2; constexpr int LENGTH_BYTE = 4; +constexpr int CONFIGURATION_SERVICE_ID = 0; constexpr int CONFIGURATION_METHOD_ID = 390; constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; 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 5c2a086a7..c0118fe82 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 @@ -199,7 +199,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE]) << 24) | (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 1]) << 16) | (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 2]) << 8) | @@ -212,13 +212,13 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector= -M_PI && msg.origin_yaw <= M_PI); assert(msg.ambiguity_free_velocity_min >= -100.f && msg.ambiguity_free_velocity_min <= 100.f); assert(msg.ambiguity_free_velocity_max >= -100.f && msg.ambiguity_free_velocity_max <= 100.f); - assert(numer_of_detections <= 800); + assert(number_of_detections <= 800); assert(msg.alignment_azimuth_correction >= -M_PI && msg.alignment_azimuth_correction <= M_PI); assert(msg.alignment_elevation_correction >= -M_PI && msg.alignment_elevation_correction <= M_PI); - msg.detections.resize(numer_of_detections); + msg.detections.resize(number_of_detections); - for (std::size_t detection_index = 0; detection_index < numer_of_detections; detection_index++) { + for (std::size_t detection_index = 0; detection_index < number_of_detections; detection_index++) { auto & detection_msg = msg.detections[detection_index]; const int CURRENT_DETECTION_START_BYTE = @@ -264,11 +264,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 1]) << 16) | (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 2]) << 8) | data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 3]; - const uint8_t invalid_flags_u = - (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_INVALID_FLAGS_BYTE + 3]; + const uint8_t invalid_flags_u = data[CURRENT_DETECTION_INVALID_FLAGS_BYTE]; const uint32_t elevation_angle_u = (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE]) << 24) | (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 1]) << 16) | @@ -317,13 +313,13 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(rcs_u); detection_msg.measurement_id = measurement_id_u; detection_msg.positive_predictive_value = positive_predictive_value_u; @@ -352,6 +348,108 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector= 0.f && detection_msg.range_rate_std <= 1.f); } + auto msg2_ptr = std::make_unique(); + auto & msg2 = *msg2_ptr; + + DetectionList detection_list; + assert(sizeof(DetectionList) == data.size()); + + std::memcpy(&detection_list, data.data(), sizeof(DetectionList)); + + msg2.header.frame_id = sensor_configuration_->frame_id; + msg2.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); + msg2.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); + msg2.stamp_sync_status = detection_list.stamp.timestamp_sync_status; + assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); + + msg2.origin_pos.x = detection_list.origin_x_pos.value(); + msg2.origin_pos.y = detection_list.origin_y_pos.value(); + msg2.origin_pos.z = detection_list.origin_z_pos.value(); + + msg2.origin_pitch = detection_list.origin_pitch.value(); + msg2.origin_pitch_std = detection_list.origin_pitch_std.value(); + msg2.origin_yaw = detection_list.origin_yaw.value(); + msg2.origin_yaw_std = detection_list.origin_yaw_std.value(); + + msg2.ambiguity_free_velocity_min = detection_list.list_rad_vel_domain_min.value(); + msg2.ambiguity_free_velocity_max = detection_list.list_rad_vel_domain_max.value(); + + msg2.alignment_azimuth_correction = detection_list.alignment_azimuth_correction.value(); + msg2.alignment_elevation_correction = detection_list.alignment_elevation_correction.value(); + + msg2.alignment_status = detection_list.alignment_status; + + const uint32_t number_of_detections2 = detection_list.number_of_detections.value(); + msg2.detections.resize(number_of_detections2); + + assert(msg2.origin_pos.x >= -10.f && msg2.origin_pos.x <= 10.f); + assert(msg2.origin_pos.y >= -10.f && msg2.origin_pos.y <= 10.f); + assert(msg2.origin_pos.z >= -10.f && msg2.origin_pos.z <= 10.f); + assert(msg2.origin_pitch >= -M_PI && msg2.origin_pitch <= M_PI); + assert(msg2.origin_yaw >= -M_PI && msg2.origin_yaw <= M_PI); + assert(msg2.ambiguity_free_velocity_min >= -100.f && msg2.ambiguity_free_velocity_min <= 100.f); + assert(msg2.ambiguity_free_velocity_max >= -100.f && msg2.ambiguity_free_velocity_max <= 100.f); + assert(number_of_detections2 <= 800); + assert(msg2.alignment_azimuth_correction >= -M_PI && msg2.alignment_azimuth_correction <= M_PI); + assert( + msg2.alignment_elevation_correction >= -M_PI && msg2.alignment_elevation_correction <= M_PI); + + for (std::size_t detection_index = 0; detection_index < number_of_detections2; + detection_index++) { + auto & detection_msg = msg2.detections[detection_index]; + auto & detection = detection_list.detections[detection_index]; + + assert(detection.positive_predictive_value <= 100); + assert(detection.classification <= 4 || detection.classification == 255); + assert(detection.multi_target_probability <= 100); + assert(detection.ambiguity_flag <= 100); + + assert(detection.azimuth_angle.value() >= -M_PI && detection.azimuth_angle.value() <= M_PI); + assert( + detection.azimuth_angle_std.value() >= 0.f && detection.azimuth_angle_std.value() <= 1.f); + assert(detection.elevation_angle.value() >= -M_PI && detection.elevation_angle.value() <= M_PI); + assert( + detection.elevation_angle_std.value() >= 0.f && detection.elevation_angle_std.value() <= 1.f); + + assert(detection.range.value() >= 0.f && detection.range.value() <= 1500.f); + assert(detection.range_std.value() >= 0.f && detection.range_std.value() <= 1.f); + assert(detection.range_rate_std.value() >= 0.f && detection.range_rate_std.value() <= 1.f); + + detection_msg.invalid_distance = detection.invalid_flags & 0x01; + detection_msg.invalid_distance_std = detection.invalid_flags & 0x02; + detection_msg.invalid_azimuth = detection.invalid_flags & 0x04; + detection_msg.invalid_azimuth_std = detection.invalid_flags & 0x08; + detection_msg.invalid_elevation = detection.invalid_flags & 0x10; + detection_msg.invalid_elevation_std = detection.invalid_flags & 0x20; + detection_msg.invalid_range_rate = detection.invalid_flags & 0x40; + detection_msg.invalid_range_rate_std = detection.invalid_flags & 0x80; + detection_msg.rcs = detection.rcs; + detection_msg.measurement_id = detection.measurement_id.value(); + detection_msg.positive_predictive_value = detection.positive_predictive_value; + detection_msg.classification = detection.classification; + detection_msg.multi_target_probability = detection.multi_target_probability; + detection_msg.object_id = detection.object_id.value(); + detection_msg.ambiguity_flag = detection.ambiguity_flag; + + detection_msg.azimuth_angle = detection.azimuth_angle.value(); + detection_msg.azimuth_angle_std = detection.azimuth_angle_std.value(); + detection_msg.elevation_angle = detection.elevation_angle.value(); + detection_msg.elevation_angle_std = detection.elevation_angle_std.value(); + + detection_msg.range = detection.range.value(); + detection_msg.range_std = detection.range_std.value(); + detection_msg.range_rate = detection.range_rate.value(); + detection_msg.range_rate_std = detection.range_rate_std.value(); + } + + assert(number_of_detections2 == number_of_detections); + + for (std::size_t i = 0; i < number_of_detections; i++) { + assert(msg.detections[i] == msg2.detections[i]); + } + + assert(msg == msg2); + detection_list_callback_(std::move(msg_ptr)); return true; @@ -375,11 +473,11 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector msg.stamp_sync_status = data[OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); - const uint8_t numer_of_objects = data[OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE]; + const uint8_t number_of_objects = data[OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE]; - msg.objects.resize(numer_of_objects); + msg.objects.resize(number_of_objects); - for (std::size_t object_index = 0; object_index < numer_of_objects; object_index++) { + for (std::size_t object_index = 0; object_index < number_of_objects; object_index++) { auto & object_msg = msg.objects[object_index]; const int CURRENT_OBJECT_START_BYTE = @@ -881,6 +979,134 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector object_msg.shape_width_edge_mean = shape_width_edge_mean_f; } + auto msg2_ptr = std::make_unique(); + auto & msg2 = *msg2_ptr; + + ObjectList object_list; + assert(sizeof(ObjectList) == data.size()); + + std::memcpy(&object_list, data.data(), sizeof(object_list)); + + msg2.header.frame_id = sensor_configuration_->frame_id; + msg2.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); + msg2.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); + msg2.stamp_sync_status = object_list.stamp.timestamp_sync_status; + assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); + + const uint8_t number_of_objects2 = object_list.number_of_objects; + + msg2.objects.resize(number_of_objects2); + + for (std::size_t object_index = 0; object_index < number_of_objects2; object_index++) { + auto & object_msg = msg2.objects[object_index]; + const Object & object = object_list.objects[object_index]; + + assert(object.status_measurement <= 2 || object.status_measurement == 255); + assert(object.status_movement <= 1 || object.status_movement == 255); + assert(object.position_reference <= 7 || object.position_reference == 255); + + assert(object.position_x.value() >= -1600.f && object.position_x.value() <= 1600.f); + assert(object.position_x_std.value() >= 0.f); + assert(object.position_y.value() >= -1600.f && object.position_y.value() <= 1600.f); + assert(object.position_y_std.value() >= 0.f); + assert(object.position_z.value() >= -1600.f && object.position_z.value() <= 1600.f); + assert(object.position_z_std.value() >= 0.f); + assert( + object.position_orientation.value() >= -M_PI && object.position_orientation.value() <= M_PI); + assert(object.position_orientation_std.value() >= 0.f); + + assert(object.classification_car <= 100); + assert(object.classification_truck <= 100); + assert(object.classification_motorcycle <= 100); + assert(object.classification_bicycle <= 100); + assert(object.classification_pedestrian <= 100); + assert(object.classification_animal <= 100); + assert(object.classification_hazard <= 100); + assert(object.classification_unknown <= 100); + + assert(object.dynamics_abs_vel_x_std.value() >= 0.f); + assert(object.dynamics_abs_vel_y_std.value() >= 0.f); + + assert(object.dynamics_rel_vel_x_std.value() >= 0.f); + assert(object.dynamics_rel_vel_y_std.value() >= 0.f); + + assert(object.dynamics_abs_accel_x_std.value() >= 0.f); + assert(object.dynamics_abs_accel_y_std.value() >= 0.f); + + assert(object.dynamics_rel_accel_x_std.value() >= 0.f); + assert(object.dynamics_rel_accel_y_std.value() >= 0.f); + + object_msg.object_id = object.id.value(); + object_msg.age = object.age.value(); + object_msg.status_measurement = object.status_measurement; + object_msg.status_movement = object.status_movement; + object_msg.position_reference = object.position_reference; + + object_msg.position.x = static_cast(object.position_x.value()); + object_msg.position.y = static_cast(object.position_y.value()); + object_msg.position.y = static_cast(object.position_z.value()); + + object_msg.position_std.x = static_cast(object.position_x_std.value()); + object_msg.position_std.y = static_cast(object.position_y_std.value()); + object_msg.position_std.z = static_cast(object.position_z_std.value()); + + object_msg.position_covariance_xy = object.position_covariance_xy.value(); + + object_msg.orientation = object.position_orientation.value(); + object_msg.orientation_std = object.position_orientation_std.value(); + + object_msg.existence_probability = object.existance_probability.value(); + object_msg.classification_car = object.classification_car; + object_msg.classification_truck = object.classification_truck; + object_msg.classification_motorcycle = object.classification_motorcycle; + object_msg.classification_bicycle = object.classification_bicycle; + object_msg.classification_pedestrian = object.classification_pedestrian; + object_msg.classification_animal = object.classification_animal; + object_msg.classification_hazard = object.classification_hazard; + object_msg.classification_unknown = object.classification_unknown; + + object_msg.absolute_velocity.x = static_cast(object.dynamics_abs_vel_x.value()); + object_msg.absolute_velocity.y = static_cast(object.dynamics_abs_vel_y.value()); + object_msg.absolute_velocity_std.x = static_cast(object.dynamics_abs_vel_x_std.value()); + object_msg.absolute_velocity_std.y = static_cast(object.dynamics_abs_vel_y_std.value()); + object_msg.absolute_velocity_covariance_xy = object.dynamics_abs_vel_covariance_xy.value(); + + object_msg.relative_velocity.x = static_cast(object.dynamics_rel_vel_x.value()); + object_msg.relative_velocity.y = static_cast(object.dynamics_rel_vel_y.value()); + object_msg.relative_velocity_std.x = static_cast(object.dynamics_rel_vel_x_std.value()); + object_msg.relative_velocity_std.y = static_cast(object.dynamics_rel_vel_y_std.value()); + object_msg.relative_velocity_covariance_xy = object.dynamics_rel_vel_covariance_xy.value(); + + object_msg.absolute_acceleration.x = static_cast(object.dynamics_abs_accel_x.value()); + object_msg.absolute_acceleration.y = static_cast(object.dynamics_abs_accel_y.value()); + object_msg.absolute_acceleration_std.x = + static_cast(object.dynamics_abs_accel_x_std.value()); + object_msg.absolute_acceleration_std.y = + static_cast(object.dynamics_abs_accel_y_std.value()); + object_msg.absolute_acceleration_covariance_xy = + object.dynamics_abs_accel_covariance_xy.value(); + + object_msg.relative_velocity.x = object.dynamics_rel_accel_x.value(); + object_msg.relative_velocity.y = object.dynamics_rel_accel_y.value(); + object_msg.relative_velocity_std.x = object.dynamics_rel_accel_x_std.value(); + object_msg.relative_velocity_std.y = object.dynamics_rel_accel_y_std.value(); + object_msg.relative_velocity_covariance_xy = object.dynamics_rel_accel_covariance_xy.value(); + + object_msg.orientation_rate_mean = object.dynamics_orientation_rate_mean.value(); + object_msg.orientation_rate_std = object.dynamics_orientation_rate_std.value(); + + object_msg.shape_length_edge_mean = object.shape_length_edge_mean.value(); + object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); + } + + assert(number_of_objects2 == number_of_objects); + + for (std::size_t i = 0; i < number_of_objects; i++) { + assert(msg.objects[i] == msg2.objects[i]); + } + + assert(msg == msg2); + object_list_callback_(std::move(msg_ptr)); return true; 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 44ce05a95..519724718 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 @@ -114,19 +114,26 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< */ - if (buffer.size() < LENGTH_BYTE + sizeof(uint32_t)) { + if (buffer.size() < sizeof(Header)) { PrintError("Unrecognized packet. Too short"); return; } - const int service_id = + Header header{}; + std::memcpy(&header, buffer.data(), sizeof(Header)); + + const uint16_t service_id = (static_cast(buffer[SERVICE_ID_BYTE]) << 8) | buffer[SERVICE_ID_BYTE + 1]; - const int method_id = + const uint16_t method_id = (static_cast(buffer[METHOD_ID_BYTE]) << 8) | buffer[METHOD_ID_BYTE + 1]; - const int length = (static_cast(buffer[LENGTH_BYTE]) << 24) | - (static_cast(buffer[LENGTH_BYTE + 1]) << 16) | - (static_cast(buffer[LENGTH_BYTE + 2]) << 8) | - buffer[LENGTH_BYTE + 3]; + const uint32_t length = (static_cast(buffer[LENGTH_BYTE]) << 24) | + (static_cast(buffer[LENGTH_BYTE + 1]) << 16) | + (static_cast(buffer[LENGTH_BYTE + 2]) << 8) | + buffer[LENGTH_BYTE + 3]; + + assert(header.service_id.value() == service_id); + assert(header.method_id.value() == method_id); + assert(header.length.value() == length); if (service_id != 0) { PrintError("Invalid service id"); @@ -175,6 +182,13 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector(buffer[STATUS_LONGITUDINAL_BYTE]) << 24) | (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 1]) << 16) | @@ -263,6 +280,7 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector> 4; @@ -409,13 +465,14 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { - // Unused available data - // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; - // constexpr int FILTER_STATUS_TIMESTAMP_SECONDS_BYTE = 12; - // constexpr int FILTER_STATUS_SYNC_STATUS_BYTE = 16; - // constexpr int FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE = 17; - // constexpr int FILTER_STATUS_DETECTION_SORT_INDEX_BYTE = 18; - // constexpr int FILTER_STATUS_OBJECT_SORT_INDEX_BYTE = 19; + // assert(false); // it seems 548 does not have this anymore.... + // Unused available data + // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; + // constexpr int FILTER_STATUS_TIMESTAMP_SECONDS_BYTE = 12; + // constexpr int FILTER_STATUS_SYNC_STATUS_BYTE = 16; + // constexpr int FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE = 17; + // constexpr int FILTER_STATUS_DETECTION_SORT_INDEX_BYTE = 18; + // constexpr int FILTER_STATUS_OBJECT_SORT_INDEX_BYTE = 19; constexpr int FILTER_STATUS_DETECTION_FILTER_BYTE = 20; constexpr int FILTER_STATUS_OBJECT_FILTER_BYTE = 90; @@ -579,6 +636,23 @@ Status ContinentalARS548HwInterface::SetSensorMounting( send_vector[CONFIGURATION_PLUG_ORIENTATION_BYTE] = plug_orientation; send_vector[CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE] = 1; + Configuration configuration{}; + assert(send_vector.size() == sizeof(Configuration)); + configuration.header.service_id = CONFIGURATION_SERVICE_ID; + configuration.header.method_id = CONFIGURATION_METHOD_ID; + configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration.configuration.longitudinal = longitudinal_autosar; + configuration.configuration.lateral = lateral_autosar; + configuration.configuration.vertical = vertical_autosar; + configuration.configuration.yaw = yaw_autosar; + configuration.configuration.pitch = pitch_autosar; + configuration.configuration.plug_orientation = plug_orientation; + configuration.new_sensor_mounting = 1; + + std::vector send_vector2(sizeof(Configuration)); + std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -640,6 +714,21 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( send_vector[CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE] = 1; + Configuration configuration{}; + assert(send_vector.size() == sizeof(Configuration)); + configuration.header.service_id = CONFIGURATION_SERVICE_ID; + configuration.header.method_id = CONFIGURATION_METHOD_ID; + configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration.configuration.length = length_autosar; + configuration.configuration.width = width_autosar; + configuration.configuration.height = height_autosar; + configuration.configuration.wheelbase = wheel_base_autosar; + configuration.new_vehicle_parameters = 1; + + std::vector send_vector2(sizeof(Configuration)); + std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -686,6 +775,22 @@ Status ContinentalARS548HwInterface::SetRadarParameters( send_vector[CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE] = 1; + Configuration configuration{}; + assert(send_vector.size() == sizeof(Configuration)); + configuration.header.service_id = CONFIGURATION_SERVICE_ID; + configuration.header.method_id = CONFIGURATION_METHOD_ID; + configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration.configuration.maximum_distance = maximum_distance; + configuration.configuration.frequency_slot = frequency_slot; + configuration.configuration.cycle_time = cycle_time; + configuration.configuration.hcc = hcc; + configuration.configuration.powersave_standstill = power_save_standstill; + configuration.new_radar_parameters = 1; + + std::vector send_vector2(sizeof(Configuration)); + std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -728,6 +833,25 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens send_vector[CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE] = 1; + Configuration configuration{}; + assert(send_vector.size() == sizeof(Configuration)); + configuration.header.service_id = CONFIGURATION_SERVICE_ID; + configuration.header.method_id = CONFIGURATION_METHOD_ID; + configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration.configuration.sensor_ip_address00 = ip_bytes[0]; + configuration.configuration.sensor_ip_address01 = ip_bytes[1]; + configuration.configuration.sensor_ip_address02 = ip_bytes[2]; + configuration.configuration.sensor_ip_address03 = ip_bytes[3]; + configuration.configuration.sensor_ip_address10 = 169; + configuration.configuration.sensor_ip_address11 = 254; + configuration.configuration.sensor_ip_address12 = 116; + configuration.configuration.sensor_ip_address13 = 113; + configuration.new_network_configuration = 1; + + std::vector send_vector2(sizeof(Configuration)); + std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -758,6 +882,17 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc send_vector[16] = bytes[1]; send_vector[17] = bytes[0]; + AccelerationLateralCoG acceleration_lateral_cog{}; + assert(send_vector.size() == sizeof(AccelerationLateralCoG)); + acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; + acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; + acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; + acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; + + std::vector send_vector2(sizeof(AccelerationLateralCoG)); + std::memcpy(send_vector2.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoG)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -788,6 +923,18 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit send_vector[16] = bytes[1]; send_vector[17] = bytes[0]; + AccelerationLongitudinalCoG acceleration_longitudinal_cog{}; + assert(send_vector.size() == sizeof(AccelerationLongitudinalCoG)); + acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; + acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; + acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; + acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; + + std::vector send_vector2(sizeof(AccelerationLongitudinalCoG)); + std::memcpy( + send_vector2.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoG)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -812,6 +959,17 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic send_vector[7] = CHARACTERISTIC_SPEED_LENGTH; send_vector[10] = static_cast(characteristic_speed); + CharasteristicSpeed characteristic_speed_packet{}; + assert(send_vector.size() == sizeof(CharasteristicSpeed)); + characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; + characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; + characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; + characteristic_speed_packet.characteristic_speed = characteristic_speed; + + std::vector send_vector2(sizeof(CharasteristicSpeed)); + std::memcpy(send_vector2.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeed)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -841,6 +999,24 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) send_vector[9] = DRIVING_DIRECTION_BACKWARDS; } + DrivingDirection driving_direction_packet{}; + assert(send_vector.size() == sizeof(DrivingDirection)); + driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; + driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; + driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; + + if (direction == 0) { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; + } else if (direction > 0) { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; + } else { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; + } + + std::vector send_vector2(sizeof(DrivingDirection)); + std::memcpy(send_vector2.data(), &driving_direction_packet, sizeof(DrivingDirection)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -871,6 +1047,18 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) send_vector[16] = bytes[1]; send_vector[17] = bytes[0]; + SteeringAngleFrontAxle steering_angle_front_axle_packet{}; + assert(send_vector.size() == sizeof(SteeringAngleFrontAxle)); + steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; + steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; + steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; + steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; + + std::vector send_vector2(sizeof(SteeringAngleFrontAxle)); + std::memcpy( + send_vector2.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxle)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -896,6 +1084,17 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) send_vector[13] = bytes[1]; send_vector[14] = bytes[0]; + VelocityVehicle steering_angle_front_axle_packet{}; + assert(send_vector.size() == sizeof(VelocityVehicle)); + steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; + steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; + steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; + steering_angle_front_axle_packet.velocity_vehicle = velocity; + + std::vector send_vector2(sizeof(VelocityVehicle)); + std::memcpy(send_vector2.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehicle)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -921,6 +1120,17 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) send_vector[16] = bytes[1]; send_vector[17] = bytes[0]; + YawRate yaw_rate_packet{}; + assert(send_vector.size() == sizeof(YawRate)); + yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; + yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; + yaw_rate_packet.header.length = YAW_RATE_LENGTH; + yaw_rate_packet.yaw_rate = yaw_rate; + + std::vector send_vector2(sizeof(YawRate)); + std::memcpy(send_vector2.data(), &yaw_rate_packet, sizeof(YawRate)); + assert(send_vector2 == send_vector2); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; From 475a057fb8c77710cb3f93447df8dc4f5bcdf03b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 20 Dec 2023 17:13:13 +0900 Subject: [PATCH 14/42] chore: removed the old implementation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 337 ++----- .../decoders/continental_ars548_decoder.cpp | 903 +----------------- .../continental_ars548_hw_interface.hpp | 15 +- .../continental_types.hpp | 28 +- .../continental_ars548_hw_interface.cpp | 864 ++++------------- ...nental_ars548_hw_interface_ros_wrapper.cpp | 14 +- 6 files changed, 388 insertions(+), 1773 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index e728a961e..a9de08fa1 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -32,18 +32,46 @@ namespace drivers namespace continental_ars548 { -using namespace boost::endian; +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; + +constexpr int CONFIGURATION_SERVICE_ID = 0; +constexpr int CONFIGURATION_METHOD_ID = 390; +constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; +constexpr int CONFIGURATION_UDP_LENGTH = 64; + +constexpr int DETECTION_LIST_METHOD_ID = 336; +constexpr int OBJECT_LIST_METHOD_ID = 329; +constexpr int SENSOR_STATUS_METHOD_ID = 380; +constexpr int FILTER_STATUS_METHOD_ID = 396; + +constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; +constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; +constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; +constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; + +constexpr int DETECTION_LIST_PDU_LENGTH = 35328; +constexpr int OBJECT_LIST_PDU_LENGTH = 9393; +constexpr int SENSOR_STATUS_PDU_LENGTH = 76; +constexpr int FILTER_STATUS_PDU_LENGTH = 322; + +constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; +constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; +constexpr int MAX_DETECTIONS = 800; +constexpr int MAX_OBJECTS = 50; #pragma pack(push, 1) -struct Header +struct HeaderPacket { big_uint16_buf_t service_id; big_uint16_buf_t method_id; big_uint32_buf_t length; }; -struct HeaderSOMEIP +struct HeaderSOMEIPPacket { big_uint16_buf_t client_id; big_uint16_buf_t session_id; @@ -53,7 +81,7 @@ struct HeaderSOMEIP uint8_t return_code; }; -struct HeaderE2EP07 +struct HeaderE2EP07Packet { big_uint64_buf_t crc; big_uint32_buf_t length; @@ -61,15 +89,15 @@ struct HeaderE2EP07 big_uint32_buf_t data_id; }; -struct StampSyncStatus +struct StampSyncStatusPacket { big_uint32_buf_t timestamp_nanoseconds; big_uint32_buf_t timestamp_seconds; uint8_t timestamp_sync_status; }; -struct Detection -{ // Actual 44 . Datasheet is 44 +struct DetectionPacket +{ big_float32_buf_t azimuth_angle; big_float32_buf_t azimuth_angle_std; uint8_t invalid_flags; @@ -89,12 +117,12 @@ struct Detection big_uint16_buf_t sort_index; }; -struct DetectionList +struct DetectionListPacket { - Header header; - HeaderSOMEIP header_someip; - HeaderE2EP07 header_e2ep07; - StampSyncStatus stamp; + HeaderPacket header; + HeaderSOMEIPPacket header_someip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; uint8_t extended_qualifier; big_uint16_buf_t origin_invalid_flags; @@ -111,7 +139,7 @@ struct DetectionList big_float32_buf_t origin_yaw; big_float32_buf_t origin_yaw_std; uint8_t list_invalid_flags; - Detection detections[800]; + DetectionPacket detections[MAX_DETECTIONS]; big_float32_buf_t list_rad_vel_domain_min; big_float32_buf_t list_rad_vel_domain_max; big_uint32_buf_t number_of_detections; @@ -121,9 +149,9 @@ struct DetectionList uint8_t reserverd[14]; }; -struct Object -{ // Datasheet 187 Current 184. Datsheet: 32x40 + 16x3 + 8x21 - big_uint16_buf_t status_sensor; // Current 32x40 16x3 21x1 +struct ObjectPacket +{ + big_uint16_buf_t status_sensor; big_uint32_buf_t id; big_uint16_buf_t age; uint8_t status_measurement; @@ -189,19 +217,19 @@ struct Object big_float32_buf_t shape_width_edge_std; }; -struct ObjectList +struct ObjectListPacket { - Header header; - HeaderSOMEIP header_someip; - HeaderE2EP07 header_e2ep07; - StampSyncStatus stamp; + HeaderPacket header; + HeaderSOMEIPPacket header_someip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; uint8_t extended_qualifier; uint8_t number_of_objects; - Object objects[50]; + ObjectPacket objects[MAX_OBJECTS]; }; -struct StatusConfiguration +struct StatusConfigurationPacket { big_float32_buf_t longitudinal; big_float32_buf_t lateral; @@ -229,269 +257,114 @@ struct StatusConfiguration uint8_t sensor_ip_address13; }; -struct SensorStatus -{ // Actual 44 + 2 + 30 = 76 bytes. datasheet: 76 - Header header; - StampSyncStatus stamp; +struct SensorStatusPacket +{ + HeaderPacket header; + StampSyncStatusPacket stamp; uint8_t sw_version_major; uint8_t sw_version_minor; uint8_t sw_version_patch; - StatusConfiguration status; + StatusConfigurationPacket status; uint8_t configuration_counter; - uint8_t status_longitudinal_velocity; - uint8_t status_longitudinal_acceleration; - uint8_t status_lateral_acceleration; - uint8_t status_yaw_rate; - uint8_t status_steering_angle; - uint8_t status_driving_direction; - uint8_t status_characteristic_speed; - uint8_t status_radar_status; - uint8_t status_voltage_status; - uint8_t status_temperature_status; - uint8_t status_blockage_status; + uint8_t longitudinal_velocity_status; + uint8_t longitudinal_acceleration_status; + uint8_t lateral_acceleration_status; + uint8_t yaw_rate_status; + uint8_t steering_angle_status; + uint8_t driving_direction_status; + uint8_t characteristic_speed_status; + uint8_t radar_status; + uint8_t voltage_status; + uint8_t temperature_status; + uint8_t blockage_status; }; -struct Configuration +struct ConfigurationPacket { - Header header; - StatusConfiguration configuration; + HeaderPacket header; + StatusConfigurationPacket configuration; uint8_t new_sensor_mounting; uint8_t new_vehicle_parameters; uint8_t new_radar_parameters; uint8_t new_network_configuration; }; -struct AccelerationLateralCoG +struct AccelerationLateralCoGPacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t acceleration_lateral; uint8_t reserved1[22]; }; -struct AccelerationLongitudinalCoG +struct AccelerationLongitudinalCoGPacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t acceleration_lateral; uint8_t reserved1[22]; }; -struct CharasteristicSpeed +struct CharasteristicSpeedPacket { - Header header; + HeaderPacket header; uint8_t reserved0[2]; - big_float32_buf_t characteristic_speed; + uint8_t characteristic_speed; uint8_t reserved1[8]; }; -struct DrivingDirection +struct DrivingDirectionPacket { - Header header; + HeaderPacket header; uint8_t reserved0; - big_float32_buf_t driving_direction; + uint8_t driving_direction; uint8_t reserved1[20]; }; -struct SteeringAngleFrontAxle +struct SteeringAngleFrontAxlePacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t steering_angle_front_axle; uint8_t reserved1[22]; }; -struct VelocityVehicle +struct VelocityVehiclePacket { - Header header; + HeaderPacket header; uint8_t reserved0[3]; big_float32_buf_t velocity_vehicle; uint8_t reserved1[21]; }; -struct YawRate +struct YawRatePacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t yaw_rate; - uint8_t reserved1[221]; + uint8_t reserved1[22]; }; -#pragma pack(pop) - -constexpr int SERVICE_ID_BYTE = 0; -constexpr int METHOD_ID_BYTE = 2; -constexpr int LENGTH_BYTE = 4; - -constexpr int CONFIGURATION_SERVICE_ID = 0; -constexpr int CONFIGURATION_METHOD_ID = 390; -constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; - -constexpr int STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; -constexpr int STATUS_TIMESTAMP_SECONDS_BYTE = 12; -constexpr int STATUS_SYNC_STATUS_BYTE = 16; -constexpr int STATUS_SW_VERSION_MAJOR_BYTE = 17; -constexpr int STATUS_SW_VERSION_MINOR_BYTE = 18; -constexpr int STATUS_SW_VERSION_PATCH_BYTE = 19; - -constexpr int STATUS_LONGITUDINAL_BYTE = 20; -constexpr int STATUS_LATERAL_BYTE = 24; -constexpr int STATUS_VERTICAL_BYTE = 28; -constexpr int STATUS_YAW_BYTE = 32; -constexpr int STATUS_PITCH_BYTE = 36; - -constexpr int STATUS_PLUG_ORIENTATION_BYTE = 40; -constexpr int STATUS_LENGTH_BYTE = 41; -constexpr int STATUS_WIDTH_BYTE = 45; -constexpr int STATUS_HEIGHT_BYTE = 49; -constexpr int STATUS_WHEEL_BASE_BYTE = 53; -constexpr int STATUS_MAXIMUM_DISTANCE_BYTE = 57; -constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; -constexpr int STATUS_CYCLE_TIME_BYTE = 60; -constexpr int STATUS_TIME_SLOT_BYTE = 61; -constexpr int STATUS_HCC_BYTE = 62; -constexpr int STATUS_POWER_SAVING_STANDSTILL_BYTE = 63; -constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; -constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; -constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; -constexpr int STATUS_LONGITUDINAL_VELOCITY_BYTE = 73; -constexpr int STATUS_LONGITUDINAL_ACCELERATION_BYTE = 74; -constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; -constexpr int STATUS_YAW_RATE_BYTE = 76; -constexpr int STATUS_STEERING_ANGLE_BYTE = 77; -constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; -constexpr int STATUS_CHARACTERISTIC_SPEED_BYTE = 79; -constexpr int STATUS_RADAR_STATUS_BYTE = 80; -constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; -constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; -constexpr int STATUS_BLOCKAGE_STATUS_BYTE = 83; - -constexpr int DETECTION_LIST_METHOD_ID = 336; -constexpr int OBJECT_LIST_METHOD_ID = 329; -constexpr int SENSOR_STATUS_METHOD_ID = 380; -constexpr int FILTER_STATUS_METHOD_ID = 396; - -constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; -constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; -constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; -constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; +struct FilterStatusEntryPacket +{ + uint8_t active; + uint8_t data_index; + big_float32_buf_t min_value; + big_float32_buf_t max_value; +}; -constexpr int DETECTION_LIST_PDU_LENGTH = 35328; -constexpr int OBJECT_LIST_PDU_LENGTH = 9393; -constexpr int SENSOR_STATUS_PDU_LENGTH = 76; -constexpr int FILTER_STATUS_PDU_LENGTH = 322; +struct FilterStatusPacket +{ + HeaderPacket header; + StampSyncStatusPacket stamp; + uint8_t filter_configuration_counter; + uint8_t detection_sort_index; + uint8_t object_sort_index; + FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; + FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; +}; -constexpr int DETECTION_LIST_CRC_BYTE = 16; -constexpr int DETECTION_LIST_LENGTH_BYTE = 24; -constexpr int DETECTION_LIST_SQC_BYTE = 28; -constexpr int DETECTION_LIST_DATA_ID_BYTE = 32; -constexpr int DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; -constexpr int DETECTION_LIST_TIMESTAMP_SECONDS_BYTE = 40; -constexpr int DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; -constexpr int DETECTION_LIST_ORIGIN_X_POS_BYTE = 52; -constexpr int DETECTION_LIST_ORIGIN_Y_POS_BYTE = 60; -constexpr int DETECTION_LIST_ORIGIN_Z_POS_BYTE = 68; -constexpr int DETECTION_LIST_PITCH_BYTE = 84; -constexpr int DETECTION_LIST_PITCH_STD_BYTE = 88; -constexpr int DETECTION_LIST_YAW_BYTE = 92; -constexpr int DETECTION_LIST_YAW_STD_BYTE = 96; -constexpr int DETECTION_LIST_ARRAY_BYTE = 101; -constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE = 35301; -constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE = 35305; -constexpr int DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE = 35309; -constexpr int DETECTION_LIST_AZIMUTH_CORRECTION_BYTE = 35313; -constexpr int DETECTION_LIST_ELEVATION_CORRECTION_BYTE = 35317; -constexpr int DETECTION_LIST_ALIGNMENT_STATUS_BYTE = 35321; - -constexpr int DETECTION_STRUCT_SIZE = 44; -constexpr int DETECTION_AZIMUTH_ANGLE_BYTE = 0; -constexpr int DETECTION_AZIMUTH_ANGLE_STD_BYTE = 4; -constexpr int DETECTION_INVALID_FLAGS_BYTE = 8; -constexpr int DETECTION_ELEVATION_ANGLE_BYTE = 9; -constexpr int DETECTION_ELEVATION_ANGLE_STD_BYTE = 13; -constexpr int DETECTION_RANGE_BYTE = 17; -constexpr int DETECTION_RANGE_STD_BYTE = 21; -constexpr int DETECTION_RANGE_RATE_BYTE = 25; -constexpr int DETECTION_RANGE_RATE_STD_BYTE = 29; -constexpr int DETECTION_RCS_BYTE = 33; -constexpr int DETECTION_MEASUREMENT_ID_BYTE = 34; -constexpr int DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = 36; -constexpr int DETECTION_CLASSIFICATION_BYTE = 37; -constexpr int DETECTION_MULT_TARGET_PROBABILITY_BYTE = 38; -constexpr int DETECTION_OBJECT_ID_BYTE = 39; -constexpr int DETECTION_AMBIGUITY_FLAG_BYTE = 41; - -constexpr int OBJECT_LIST_CRC_BYTE = 16; -constexpr int OBJECT_LIST_LENGTH_BYTE = 24; -constexpr int OBJECT_LIST_SQC_BYTE = 28; -constexpr int OBJECT_LIST_DATA_ID_BYTE = 32; -constexpr int OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; -constexpr int OBJECT_LIST_TIMESTAMP_SECONDS_BYTE = 40; -constexpr int OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; -constexpr int OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE = 50; -constexpr int OBJECT_LIST_ARRAY_BYTE = 51; - -constexpr int OBJECT_STRUCT_SIZE = 187; -constexpr int OBJECT_STATUS_SENSOR_BYTE = 0; -constexpr int OBJECT_ID_BYTE = 2; -constexpr int OBJECT_AGE_BYTE = 6; -constexpr int OBJECT_STATUS_MEASUREMENT_BYTE = 8; -constexpr int OBJECT_STATUS_MOVEMENT_BYTE = 9; -constexpr int OBJECT_POSITION_REFERENCE_BYTE = 12; -constexpr int OBJECT_POSITION_X_BYTE = 13; -constexpr int OBJECT_POSITION_X_STD_BYTE = 17; -constexpr int OBJECT_POSITION_Y_BYTE = 21; -constexpr int OBJECT_POSITION_Y_STD_BYTE = 25; -constexpr int OBJECT_POSITION_Z_BYTE = 29; -constexpr int OBJECT_POSITION_Z_STD_BYTE = 33; - -constexpr int OBJECT_POSITION_COVARIANCE_XY_BYTE = 37; -constexpr int OBJECT_ORIENTATION_BYTE = 41; -constexpr int OBJECT_ORIENTATION_STD_BYTE = 45; - -constexpr int OBJECT_EXISTENCE_PROBABILITY_BYTE = 50; - -constexpr int OBJECT_CLASSIFICATION_CAR_BYTE = 58; -constexpr int OBJECT_CLASSIFICATION_TRUCK_BYTE = 59; -constexpr int OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = 60; -constexpr int OBJECT_CLASSIFICATION_BICYCLE_BYTE = 61; -constexpr int OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = 62; -constexpr int OBJECT_CLASSIFICATION_ANIMAL_BYTE = 63; -constexpr int OBJECT_CLASSIFICATION_HAZARD_BYTE = 64; -constexpr int OBJECT_CLASSIFICATION_UNKNOWN_BYTE = 65; - -constexpr int OBJECT_DYNAMICS_ABS_VEL_X_BYTE = 69; -constexpr int OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = 73; -constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = 77; -constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = 81; -constexpr int OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = 85; - -constexpr int OBJECT_DYNAMICS_REL_VEL_X_BYTE = 90; -constexpr int OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = 94; -constexpr int OBJECT_DYNAMICS_REL_VEL_Y_BYTE = 98; -constexpr int OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = 102; -constexpr int OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = 106; - -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = 111; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = 115; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = 119; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = 123; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = 127; - -constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = 132; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = 136; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = 140; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = 144; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = 148; - -constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = 153; -constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = 157; - -constexpr int OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = 166; -constexpr int OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = 179; - -static constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; -static constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; +#pragma pack(pop) } // namespace continental_ars548 } // namespace drivers 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 c0118fe82..c9e46a30d 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 @@ -57,31 +57,27 @@ bool ContinentalARS548Decoder::ProcessPackets( const auto & data = nebula_packets.packets[0].data; - if (data.size() < LENGTH_BYTE + sizeof(uint32_t)) { + if (data.size() < sizeof(HeaderPacket)) { return false; } - const uint16_t service_id = - (static_cast(data[SERVICE_ID_BYTE]) << 8) | data[SERVICE_ID_BYTE + 1]; - const uint16_t method_id = - (static_cast(data[METHOD_ID_BYTE]) << 8) | data[METHOD_ID_BYTE + 1]; - const uint32_t length = (static_cast(data[LENGTH_BYTE]) << 24) | - (static_cast(data[LENGTH_BYTE + 1]) << 16) | - (static_cast(data[LENGTH_BYTE + 2]) << 8) | - static_cast(data[LENGTH_BYTE + 3]); + HeaderPacket header{}; + std::memcpy(&header, data.data(), sizeof(HeaderPacket)); - if (service_id != 0) { + if (header.service_id.value() != 0) { return false; } - if (method_id == DETECTION_LIST_METHOD_ID) { - if (data.size() != DETECTION_LIST_UDP_PAYLOAD || length != DETECTION_LIST_PDU_LENGTH) { + if (header.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + data.size() != DETECTION_LIST_UDP_PAYLOAD || + header.length.value() != DETECTION_LIST_PDU_LENGTH) { return false; } return ParseDetectionsListPacket(data); - } else if (method_id == OBJECT_LIST_METHOD_ID) { - if (data.size() != OBJECT_LIST_UDP_PAYLOAD || length != OBJECT_LIST_PDU_LENGTH) { + } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { + if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } @@ -96,118 +92,40 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(); auto & msg = *msg_ptr; + DetectionListPacket detection_list; + assert(sizeof(DetectionListPacket) == data.size()); + + std::memcpy(&detection_list, data.data(), sizeof(DetectionListPacket)); + msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.stamp.nanosec = - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; - msg.header.stamp.sec = - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 3]; - msg.stamp_sync_status = data[DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); + msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); - const uint32_t origin_pos_x_u = - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 3]; - const uint32_t origin_pos_y_u = - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 3]; - const uint32_t origin_pos_z_u = - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 3]; - - float origin_pos_x_f, origin_pos_y_f, origin_pos_z_f; - - std::memcpy(&origin_pos_x_f, &origin_pos_x_u, sizeof(origin_pos_x_u)); - std::memcpy(&origin_pos_y_f, &origin_pos_y_u, sizeof(origin_pos_y_u)); - std::memcpy(&origin_pos_z_f, &origin_pos_z_u, sizeof(origin_pos_z_u)); - - msg.origin_pos.x = static_cast(origin_pos_x_f); - msg.origin_pos.y = static_cast(origin_pos_y_f); - msg.origin_pos.z = static_cast(origin_pos_z_f); - - const uint32_t origin_pitch_u = - (static_cast(data[DETECTION_LIST_PITCH_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_PITCH_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_PITCH_BYTE + 2]) << 8) | - data[DETECTION_LIST_PITCH_BYTE + 3]; - const uint32_t origin_pitch_std_u = - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 2]) << 8) | - data[DETECTION_LIST_PITCH_STD_BYTE + 3]; - const uint32_t origin_yaw_u = (static_cast(data[DETECTION_LIST_YAW_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_YAW_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_YAW_BYTE + 2]) << 8) | - data[DETECTION_LIST_YAW_BYTE + 3]; - const uint32_t origin_yaw_std_u = - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 2]) << 8) | - data[DETECTION_LIST_YAW_STD_BYTE + 3]; - - std::memcpy(&msg.origin_pitch, &origin_pitch_u, sizeof(origin_pitch_u)); - std::memcpy(&msg.origin_pitch_std, &origin_pitch_std_u, sizeof(origin_pitch_std_u)); - std::memcpy(&msg.origin_yaw, &origin_yaw_u, sizeof(origin_yaw_u)); - std::memcpy(&msg.origin_yaw_std, &origin_yaw_std_u, sizeof(origin_yaw_std_u)); - - const uint32_t ambiguity_free_velocity_min_u = - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 2]) << 8) | - data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 3]; - const uint32_t ambiguity_free_velocity_max_u = - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 2]) << 8) | - data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 3]; - - std::memcpy( - &msg.ambiguity_free_velocity_min, &ambiguity_free_velocity_min_u, - sizeof(ambiguity_free_velocity_min_u)); - std::memcpy( - &msg.ambiguity_free_velocity_max, &ambiguity_free_velocity_max_u, - sizeof(ambiguity_free_velocity_max_u)); - - const uint32_t alignment_azimuth_correction_u = - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 2]) << 8) | - data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 3]; - const uint32_t alignment_elevation_correction_u = - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 2]) << 8) | - data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 3]; - - std::memcpy( - &msg.alignment_azimuth_correction, &alignment_azimuth_correction_u, - sizeof(alignment_azimuth_correction_u)); - std::memcpy( - &msg.alignment_elevation_correction, &alignment_elevation_correction_u, - sizeof(alignment_elevation_correction_u)); - - msg.alignment_status = data[DETECTION_LIST_ALIGNMENT_STATUS_BYTE]; - - const uint32_t number_of_detections = - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 2]) << 8) | - data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 3]; - - assert(origin_pos_x_f >= -10.f && origin_pos_x_f <= 10.f); - assert(origin_pos_y_f >= -10.f && origin_pos_y_f <= 10.f); - assert(origin_pos_z_f >= -10.f && origin_pos_z_f <= 10.f); + msg.origin_pos.x = detection_list.origin_x_pos.value(); + msg.origin_pos.y = detection_list.origin_y_pos.value(); + msg.origin_pos.z = detection_list.origin_z_pos.value(); + + msg.origin_pitch = detection_list.origin_pitch.value(); + msg.origin_pitch_std = detection_list.origin_pitch_std.value(); + msg.origin_yaw = detection_list.origin_yaw.value(); + msg.origin_yaw_std = detection_list.origin_yaw_std.value(); + + msg.ambiguity_free_velocity_min = detection_list.list_rad_vel_domain_min.value(); + msg.ambiguity_free_velocity_max = detection_list.list_rad_vel_domain_max.value(); + + msg.alignment_azimuth_correction = detection_list.alignment_azimuth_correction.value(); + msg.alignment_elevation_correction = detection_list.alignment_elevation_correction.value(); + + msg.alignment_status = detection_list.alignment_status; + + const uint32_t number_of_detections = detection_list.number_of_detections.value(); + msg.detections.resize(number_of_detections); + + assert(msg.origin_pos.x >= -10.f && msg.origin_pos.x <= 10.f); + assert(msg.origin_pos.y >= -10.f && msg.origin_pos.y <= 10.f); + assert(msg.origin_pos.z >= -10.f && msg.origin_pos.z <= 10.f); assert(msg.origin_pitch >= -M_PI && msg.origin_pitch <= M_PI); assert(msg.origin_yaw >= -M_PI && msg.origin_yaw <= M_PI); assert(msg.ambiguity_free_velocity_min >= -100.f && msg.ambiguity_free_velocity_min <= 100.f); @@ -216,187 +134,8 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector= -M_PI && msg.alignment_azimuth_correction <= M_PI); assert(msg.alignment_elevation_correction >= -M_PI && msg.alignment_elevation_correction <= M_PI); - msg.detections.resize(number_of_detections); - for (std::size_t detection_index = 0; detection_index < number_of_detections; detection_index++) { auto & detection_msg = msg.detections[detection_index]; - - const int CURRENT_DETECTION_START_BYTE = - DETECTION_LIST_ARRAY_BYTE + detection_index * DETECTION_STRUCT_SIZE; - const int CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_BYTE; - const int CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_STD_BYTE; - const int CURRENT_DETECTION_INVALID_FLAGS_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_INVALID_FLAGS_BYTE; - const int CURRENT_DETECTION_ELEVATION_ANGLE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_BYTE; - const int CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_STD_BYTE; - const int CURRENT_DETECTION_RANGE_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_BYTE; - const int CURRENT_DETECTION_RANGE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_STD_BYTE; - const int CURRENT_DETECTION_RANGE_RATE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_BYTE; - const int CURRENT_DETECTION_RANGE_RATE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_STD_BYTE; - const int CURRENT_DETECTION_RCS_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RCS_BYTE; - const int CURRENT_DETECTION_MEASUREMENT_ID_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_MEASUREMENT_ID_BYTE; - const int CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE; - const int CURRENT_DETECTION_CLASSIFICATION_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_CLASSIFICATION_BYTE; - const int CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_MULT_TARGET_PROBABILITY_BYTE; - const int CURRENT_DETECTION_OBJECT_ID_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_OBJECT_ID_BYTE; - const int CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AMBIGUITY_FLAG_BYTE; - - const uint32_t azimuth_angle_u = - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 3]; - const uint32_t azimuth_angle_std_u = - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 3]; - const uint8_t invalid_flags_u = data[CURRENT_DETECTION_INVALID_FLAGS_BYTE]; - const uint32_t elevation_angle_u = - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 3]; - const uint32_t elevation_angle_std_u = - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 3]; - const uint32_t range_u = (static_cast(data[CURRENT_DETECTION_RANGE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_BYTE + 3]; - const uint32_t range_std_u = - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_STD_BYTE + 3]; - const uint32_t range_rate_u = - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_RATE_BYTE + 3]; - const uint32_t range_rate_std_u = - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 3]; - const uint8_t rcs_u = data[CURRENT_DETECTION_RCS_BYTE]; - const uint16_t measurement_id_u = - (static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE]) << 8) | - static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE + 1]); - const uint8_t positive_predictive_value_u = - data[CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE]; - const uint8_t classification_u = data[CURRENT_DETECTION_CLASSIFICATION_BYTE]; - const uint8_t multi_target_probability_u = data[CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE]; - const uint16_t object_id_u = - (static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE]) << 8) | - static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE + 1]); - const uint8_t ambiguity_flag_u = data[CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE]; - - assert(positive_predictive_value_u <= 100); - assert(classification_u <= 4 || classification_u == 255); - assert(multi_target_probability_u <= 100); - assert(ambiguity_flag_u <= 100); - - detection_msg.invalid_distance = invalid_flags_u & 0x01; - detection_msg.invalid_distance_std = invalid_flags_u & 0x02; - detection_msg.invalid_azimuth = invalid_flags_u & 0x04; - detection_msg.invalid_azimuth_std = invalid_flags_u & 0x08; - detection_msg.invalid_elevation = invalid_flags_u & 0x10; - detection_msg.invalid_elevation_std = invalid_flags_u & 0x20; - detection_msg.invalid_range_rate = invalid_flags_u & 0x40; - detection_msg.invalid_range_rate_std = invalid_flags_u & 0x80; - detection_msg.rcs = static_cast(rcs_u); - detection_msg.measurement_id = measurement_id_u; - detection_msg.positive_predictive_value = positive_predictive_value_u; - detection_msg.classification = classification_u; - detection_msg.multi_target_probability = multi_target_probability_u; - detection_msg.object_id = object_id_u; - detection_msg.ambiguity_flag = ambiguity_flag_u; - std::memcpy(&detection_msg.azimuth_angle, &azimuth_angle_u, sizeof(azimuth_angle_u)); - std::memcpy( - &detection_msg.azimuth_angle_std, &azimuth_angle_std_u, sizeof(azimuth_angle_std_u)); - std::memcpy(&detection_msg.elevation_angle, &elevation_angle_u, sizeof(elevation_angle_u)); - std::memcpy( - &detection_msg.elevation_angle_std, &elevation_angle_std_u, sizeof(elevation_angle_std_u)); - std::memcpy(&detection_msg.range, &range_u, sizeof(range_u)); - std::memcpy(&detection_msg.range_std, &range_std_u, sizeof(range_std_u)); - std::memcpy(&detection_msg.range_rate, &range_rate_u, sizeof(range_rate_u)); - std::memcpy(&detection_msg.range_rate_std, &range_rate_std_u, sizeof(range_rate_std_u)); - - assert(detection_msg.azimuth_angle >= -M_PI && detection_msg.azimuth_angle <= M_PI); - assert(detection_msg.azimuth_angle_std >= 0.f && detection_msg.azimuth_angle_std <= 1.f); - assert(detection_msg.elevation_angle >= -M_PI && detection_msg.elevation_angle <= M_PI); - assert(detection_msg.elevation_angle_std >= 0.f && detection_msg.elevation_angle_std <= 1.f); - - assert(detection_msg.range >= 0.f && detection_msg.range <= 1500.f); - assert(detection_msg.range_std >= 0.f && detection_msg.range_std <= 1.f); - assert(detection_msg.range_rate_std >= 0.f && detection_msg.range_rate_std <= 1.f); - } - - auto msg2_ptr = std::make_unique(); - auto & msg2 = *msg2_ptr; - - DetectionList detection_list; - assert(sizeof(DetectionList) == data.size()); - - std::memcpy(&detection_list, data.data(), sizeof(DetectionList)); - - msg2.header.frame_id = sensor_configuration_->frame_id; - msg2.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); - msg2.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); - msg2.stamp_sync_status = detection_list.stamp.timestamp_sync_status; - assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); - - msg2.origin_pos.x = detection_list.origin_x_pos.value(); - msg2.origin_pos.y = detection_list.origin_y_pos.value(); - msg2.origin_pos.z = detection_list.origin_z_pos.value(); - - msg2.origin_pitch = detection_list.origin_pitch.value(); - msg2.origin_pitch_std = detection_list.origin_pitch_std.value(); - msg2.origin_yaw = detection_list.origin_yaw.value(); - msg2.origin_yaw_std = detection_list.origin_yaw_std.value(); - - msg2.ambiguity_free_velocity_min = detection_list.list_rad_vel_domain_min.value(); - msg2.ambiguity_free_velocity_max = detection_list.list_rad_vel_domain_max.value(); - - msg2.alignment_azimuth_correction = detection_list.alignment_azimuth_correction.value(); - msg2.alignment_elevation_correction = detection_list.alignment_elevation_correction.value(); - - msg2.alignment_status = detection_list.alignment_status; - - const uint32_t number_of_detections2 = detection_list.number_of_detections.value(); - msg2.detections.resize(number_of_detections2); - - assert(msg2.origin_pos.x >= -10.f && msg2.origin_pos.x <= 10.f); - assert(msg2.origin_pos.y >= -10.f && msg2.origin_pos.y <= 10.f); - assert(msg2.origin_pos.z >= -10.f && msg2.origin_pos.z <= 10.f); - assert(msg2.origin_pitch >= -M_PI && msg2.origin_pitch <= M_PI); - assert(msg2.origin_yaw >= -M_PI && msg2.origin_yaw <= M_PI); - assert(msg2.ambiguity_free_velocity_min >= -100.f && msg2.ambiguity_free_velocity_min <= 100.f); - assert(msg2.ambiguity_free_velocity_max >= -100.f && msg2.ambiguity_free_velocity_max <= 100.f); - assert(number_of_detections2 <= 800); - assert(msg2.alignment_azimuth_correction >= -M_PI && msg2.alignment_azimuth_correction <= M_PI); - assert( - msg2.alignment_elevation_correction >= -M_PI && msg2.alignment_elevation_correction <= M_PI); - - for (std::size_t detection_index = 0; detection_index < number_of_detections2; - detection_index++) { - auto & detection_msg = msg2.detections[detection_index]; auto & detection = detection_list.detections[detection_index]; assert(detection.positive_predictive_value <= 100); @@ -442,14 +181,6 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; + + ObjectListPacket object_list; + assert(sizeof(ObjectListPacket) == data.size()); + + std::memcpy(&object_list, data.data(), sizeof(object_list)); + msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.stamp.nanosec = - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; - msg.header.stamp.sec = - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 3]; - msg.stamp_sync_status = data[OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); + msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); - const uint8_t number_of_objects = data[OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE]; + const uint8_t number_of_objects = object_list.number_of_objects; msg.objects.resize(number_of_objects); for (std::size_t object_index = 0; object_index < number_of_objects; object_index++) { auto & object_msg = msg.objects[object_index]; - - const int CURRENT_OBJECT_START_BYTE = - OBJECT_LIST_ARRAY_BYTE + object_index * OBJECT_STRUCT_SIZE; - const int CURRENT_OBJECT_ID_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ID_BYTE; - const int CURRENT_OBJECT_AGE_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_AGE_BYTE; - - const int CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MEASUREMENT_BYTE; - const int CURRENT_OBJECT_STATUS_MOVEMENT_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MOVEMENT_BYTE; - const int CURRENT_OBJECT_POSITION_REFERENCE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_REFERENCE_BYTE; - const int CURRENT_OBJECT_POSITION_X_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_BYTE; - const int CURRENT_OBJECT_POSITION_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_STD_BYTE; - const int CURRENT_OBJECT_POSITION_Y_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_BYTE; - const int CURRENT_OBJECT_POSITION_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_STD_BYTE; - const int CURRENT_OBJECT_POSITION_Z_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_BYTE; - const int CURRENT_OBJECT_POSITION_Z_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_STD_BYTE; - - const int CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_COVARIANCE_XY_BYTE; - const int CURRENT_OBJECT_ORIENTATION_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_BYTE; - const int CURRENT_OBJECT_ORIENTATION_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_STD_BYTE; - - const int CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_EXISTENCE_PROBABILITY_BYTE; - - const int CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_CAR_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_TRUCK_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_BICYCLE_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_ANIMAL_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_HAZARD_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_UNKNOWN_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE; - - const int CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE; - const int CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE; - - const uint32_t object_id_u = (static_cast(data[CURRENT_OBJECT_ID_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ID_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ID_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ID_BYTE + 3]; - const uint16_t object_age_u = (static_cast(data[CURRENT_OBJECT_AGE_BYTE]) << 8) | - static_cast(data[CURRENT_OBJECT_AGE_BYTE + 1]); - const uint8_t status_measurement_u = data[CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE]; - const uint8_t status_movement_u = data[CURRENT_OBJECT_STATUS_MOVEMENT_BYTE]; - const uint8_t position_reference_u = data[CURRENT_OBJECT_POSITION_REFERENCE_BYTE]; - - const uint32_t position_x_u = - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_X_BYTE + 3]; - const uint32_t position_x_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 3]; - const uint32_t position_y_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Y_BYTE + 3]; - const uint32_t position_y_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 3]; - const uint32_t position_z_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Z_BYTE + 3]; - const uint32_t position_z_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 3]; - const uint32_t position_xy_covariance_u = - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 3]; - const uint32_t orientation_u = - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ORIENTATION_BYTE + 3]; - const uint32_t orientation_std_u = - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 3]; - const uint32_t existence_probability_u = - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 3]; - - float position_x_f; - float position_x_std_f; - float position_y_f; - float position_y_std_f; - float position_z_f; - float position_z_std_f; - float position_xy_covariance_f; - float orientation_f; - float orientation_std_f; - float existence_probability_f; - - std::memcpy(&position_x_f, &position_x_u, sizeof(position_x_u)); - std::memcpy(&position_x_std_f, &position_x_std_u, sizeof(position_x_std_u)); - std::memcpy(&position_y_f, &position_y_u, sizeof(position_y_u)); - std::memcpy(&position_y_std_f, &position_y_std_u, sizeof(position_y_std_u)); - std::memcpy(&position_z_f, &position_z_u, sizeof(position_z_u)); - std::memcpy(&position_z_std_f, &position_z_std_u, sizeof(position_z_std_u)); - std::memcpy( - &position_xy_covariance_f, &position_xy_covariance_u, sizeof(position_xy_covariance_u)); - std::memcpy(&orientation_f, &orientation_u, sizeof(orientation_u)); - std::memcpy(&orientation_std_f, &orientation_std_u, sizeof(orientation_std_u)); - std::memcpy( - &existence_probability_f, &existence_probability_u, sizeof(existence_probability_u)); - - const uint8_t classification_car_u = data[CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE]; - const uint8_t classification_truck_u = data[CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE]; - const uint8_t classification_motorcycle_u = data[CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE]; - const uint8_t classification_bicycle_u = data[CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE]; - const uint8_t classification_pedestrian_u = data[CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE]; - const uint8_t classification_animal_u = data[CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE]; - const uint8_t classification_hazard_u = data[CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE]; - const uint8_t classification_unknown_u = data[CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE]; - - const uint32_t dynamics_abs_vel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 3]; - const uint32_t dynamics_abs_vel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 3]; - const uint32_t dynamics_abs_vel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 3]; - const uint32_t dynamics_abs_vel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_abs_vel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_rel_vel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 3]; - const uint32_t dynamics_rel_vel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 3]; - const uint32_t dynamics_rel_vel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 3]; - const uint32_t dynamics_rel_vel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_rel_vel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_abs_accel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 3]; - const uint32_t dynamics_abs_accel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 3]; - const uint32_t dynamics_abs_accel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 3]; - const uint32_t dynamics_abs_accel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_abs_accel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 1]) - << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_rel_accel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 3]; - const uint32_t dynamics_rel_accel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 3]; - const uint32_t dynamics_rel_accel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 3]; - ; - const uint32_t dynamics_rel_accel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_rel_accel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 1]) - << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 3]; - - float dynamics_abs_vel_x_f; - float dynamics_abs_vel_x_std_f; - float dynamics_abs_vel_y_f; - float dynamics_abs_vel_y_std_f; - float dynamics_abs_vel_covariance_xy_f; - - float dynamics_rel_vel_x_f; - float dynamics_rel_vel_x_std_f; - float dynamics_rel_vel_y_f; - float dynamics_rel_vel_y_std_f; - float dynamics_rel_vel_covariance_xy_f; - - float dynamics_abs_accel_x_f; - float dynamics_abs_accel_x_std_f; - float dynamics_abs_accel_y_f; - float dynamics_abs_accel_y_std_f; - float dynamics_abs_accel_covariance_xy_f; - - float dynamics_rel_accel_x_f; - float dynamics_rel_accel_x_std_f; - float dynamics_rel_accel_y_f; - float dynamics_rel_accel_y_std_f; - float dynamics_rel_accel_covariance_xy_f; - - std::memcpy(&dynamics_abs_vel_x_f, &dynamics_abs_vel_x_u, sizeof(dynamics_abs_vel_x_u)); - std::memcpy( - &dynamics_abs_vel_x_std_f, &dynamics_abs_vel_x_std_u, sizeof(dynamics_abs_vel_x_std_u)); - std::memcpy(&dynamics_abs_vel_y_f, &dynamics_abs_vel_y_u, sizeof(dynamics_abs_vel_y_u)); - std::memcpy( - &dynamics_abs_vel_y_std_f, &dynamics_abs_vel_y_std_u, sizeof(dynamics_abs_vel_y_std_u)); - std::memcpy( - &dynamics_abs_vel_covariance_xy_f, &dynamics_abs_vel_covariance_xy_u, - sizeof(dynamics_abs_vel_covariance_xy_u)); - - std::memcpy(&dynamics_rel_vel_x_f, &dynamics_rel_vel_x_u, sizeof(dynamics_rel_vel_x_u)); - std::memcpy( - &dynamics_rel_vel_x_std_f, &dynamics_rel_vel_x_std_u, sizeof(dynamics_rel_vel_x_std_u)); - std::memcpy(&dynamics_rel_vel_y_f, &dynamics_rel_vel_y_u, sizeof(dynamics_rel_vel_y_u)); - std::memcpy( - &dynamics_rel_vel_y_std_f, &dynamics_rel_vel_y_std_u, sizeof(dynamics_rel_vel_y_std_u)); - std::memcpy( - &dynamics_rel_vel_covariance_xy_f, &dynamics_rel_vel_covariance_xy_u, - sizeof(dynamics_rel_vel_covariance_xy_u)); - - std::memcpy(&dynamics_abs_accel_x_f, &dynamics_abs_accel_x_u, sizeof(dynamics_abs_accel_x_u)); - std::memcpy( - &dynamics_abs_accel_x_std_f, &dynamics_abs_accel_x_std_u, sizeof(dynamics_abs_accel_x_std_u)); - std::memcpy(&dynamics_abs_accel_y_f, &dynamics_abs_accel_y_u, sizeof(dynamics_abs_accel_y_u)); - std::memcpy( - &dynamics_abs_accel_y_std_f, &dynamics_abs_accel_y_std_u, sizeof(dynamics_abs_accel_y_std_u)); - std::memcpy( - &dynamics_abs_accel_covariance_xy_f, &dynamics_abs_accel_covariance_xy_u, - sizeof(dynamics_abs_accel_covariance_xy_u)); - - std::memcpy(&dynamics_rel_accel_x_f, &dynamics_rel_accel_x_u, sizeof(dynamics_rel_accel_x_u)); - std::memcpy( - &dynamics_rel_accel_x_std_f, &dynamics_rel_accel_x_std_u, sizeof(dynamics_rel_accel_x_std_u)); - std::memcpy(&dynamics_rel_accel_y_f, &dynamics_rel_accel_y_u, sizeof(dynamics_rel_accel_y_u)); - std::memcpy( - &dynamics_rel_accel_y_std_f, &dynamics_rel_accel_y_std_u, sizeof(dynamics_rel_accel_y_std_u)); - std::memcpy( - &dynamics_rel_accel_covariance_xy_f, &dynamics_rel_accel_covariance_xy_u, - sizeof(dynamics_rel_accel_covariance_xy_u)); - - const uint32_t dynamics_orientation_rate_mean_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 3]; - const uint32_t dynamics_orientation_rate_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 3]; - - const uint32_t shape_length_edge_mean_u = - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 3]; - const uint32_t shape_width_edge_mean_u = - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 3]; - - float dynamics_orientation_rate_mean_f; - float dynamics_orientation_rate_std_f; - - float shape_length_edge_mean_f; - float shape_width_edge_mean_f; - - std::memcpy( - &dynamics_orientation_rate_mean_f, &dynamics_orientation_rate_mean_u, - sizeof(dynamics_orientation_rate_mean_u)); - std::memcpy( - &dynamics_orientation_rate_std_f, &dynamics_orientation_rate_std_u, - sizeof(dynamics_orientation_rate_std_u)); - std::memcpy( - &shape_length_edge_mean_f, &shape_length_edge_mean_u, sizeof(shape_length_edge_mean_u)); - std::memcpy( - &shape_width_edge_mean_f, &shape_width_edge_mean_u, sizeof(shape_width_edge_mean_u)); - - assert(status_measurement_u <= 2 || status_measurement_u == 255); - assert(status_movement_u <= 1 || status_movement_u == 255); - assert(position_reference_u <= 7 || position_reference_u == 255); - - assert(position_x_f >= -1600.f && position_x_f <= 1600.f); - assert(position_x_std_f >= 0.f); - assert(position_y_f >= -1600.f && position_y_f <= 1600.f); - assert(position_y_std_f >= 0.f); - assert(position_z_f >= -1600.f && position_z_f <= 1600.f); - assert(position_z_std_f >= 0.f); - assert(orientation_f >= -M_PI && orientation_f <= M_PI); - assert(orientation_std_f >= 0.f); - - assert(classification_car_u <= 100); - assert(classification_truck_u <= 100); - assert(classification_motorcycle_u <= 100); - assert(classification_bicycle_u <= 100); - assert(classification_pedestrian_u <= 100); - assert(classification_animal_u <= 100); - assert(classification_hazard_u <= 100); - assert(classification_unknown_u <= 100); - - assert(dynamics_abs_vel_x_std_f >= 0.f); - assert(dynamics_abs_vel_y_std_f >= 0.f); - - assert(dynamics_rel_vel_x_std_f >= 0.f); - assert(dynamics_rel_vel_y_std_f >= 0.f); - - assert(dynamics_abs_accel_x_std_f >= 0.f); - assert(dynamics_abs_accel_y_std_f >= 0.f); - - assert(dynamics_rel_accel_x_std_f >= 0.f); - assert(dynamics_rel_accel_y_std_f >= 0.f); - - object_msg.object_id = object_id_u; - object_msg.age = object_age_u; - object_msg.status_measurement = status_measurement_u; - object_msg.status_movement = status_movement_u; - object_msg.position_reference = position_reference_u; - - object_msg.position.x = static_cast(position_x_f); - object_msg.position.y = static_cast(position_y_f); - object_msg.position.y = static_cast(position_z_f); - - object_msg.position_std.x = static_cast(position_x_std_f); - object_msg.position_std.y = static_cast(position_y_std_f); - object_msg.position_std.z = static_cast(position_z_std_f); - - object_msg.position_covariance_xy = position_xy_covariance_f; - - object_msg.orientation = orientation_f; - object_msg.orientation_std = orientation_std_f; - - object_msg.existence_probability = existence_probability_f; - object_msg.classification_car = classification_car_u; - object_msg.classification_truck = classification_truck_u; - object_msg.classification_motorcycle = classification_motorcycle_u; - object_msg.classification_bicycle = classification_bicycle_u; - object_msg.classification_pedestrian = classification_pedestrian_u; - object_msg.classification_animal = classification_animal_u; - object_msg.classification_hazard = classification_hazard_u; - object_msg.classification_unknown = classification_unknown_u; - - object_msg.absolute_velocity.x = static_cast(dynamics_abs_vel_x_f); - object_msg.absolute_velocity.y = static_cast(dynamics_abs_vel_y_f); - object_msg.absolute_velocity_std.x = static_cast(dynamics_abs_vel_x_std_f); - object_msg.absolute_velocity_std.y = static_cast(dynamics_abs_vel_y_std_f); - object_msg.absolute_velocity_covariance_xy = dynamics_abs_vel_covariance_xy_f; - - object_msg.relative_velocity.x = static_cast(dynamics_rel_vel_x_f); - object_msg.relative_velocity.y = static_cast(dynamics_rel_vel_y_f); - object_msg.relative_velocity_std.x = static_cast(dynamics_rel_vel_x_std_f); - object_msg.relative_velocity_std.y = static_cast(dynamics_rel_vel_y_std_f); - object_msg.relative_velocity_covariance_xy = dynamics_rel_vel_covariance_xy_f; - - object_msg.absolute_acceleration.x = static_cast(dynamics_abs_accel_x_f); - object_msg.absolute_acceleration.y = static_cast(dynamics_abs_accel_y_f); - object_msg.absolute_acceleration_std.x = static_cast(dynamics_abs_accel_x_std_f); - object_msg.absolute_acceleration_std.y = static_cast(dynamics_abs_accel_y_std_f); - object_msg.absolute_acceleration_covariance_xy = dynamics_abs_accel_covariance_xy_f; - - object_msg.relative_velocity.x = dynamics_rel_accel_x_f; - object_msg.relative_velocity.y = dynamics_rel_accel_y_f; - object_msg.relative_velocity_std.x = dynamics_rel_accel_x_std_f; - object_msg.relative_velocity_std.y = dynamics_rel_accel_y_std_f; - object_msg.relative_velocity_covariance_xy = dynamics_rel_accel_covariance_xy_f; - - object_msg.orientation_rate_mean = dynamics_orientation_rate_mean_f; - object_msg.orientation_rate_std = dynamics_orientation_rate_std_f; - - object_msg.shape_length_edge_mean = shape_length_edge_mean_f; - object_msg.shape_width_edge_mean = shape_width_edge_mean_f; - } - - auto msg2_ptr = std::make_unique(); - auto & msg2 = *msg2_ptr; - - ObjectList object_list; - assert(sizeof(ObjectList) == data.size()); - - std::memcpy(&object_list, data.data(), sizeof(object_list)); - - msg2.header.frame_id = sensor_configuration_->frame_id; - msg2.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); - msg2.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); - msg2.stamp_sync_status = object_list.stamp.timestamp_sync_status; - assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); - - const uint8_t number_of_objects2 = object_list.number_of_objects; - - msg2.objects.resize(number_of_objects2); - - for (std::size_t object_index = 0; object_index < number_of_objects2; object_index++) { - auto & object_msg = msg2.objects[object_index]; - const Object & object = object_list.objects[object_index]; + const ObjectPacket & object = object_list.objects[object_index]; assert(object.status_measurement <= 2 || object.status_measurement == 255); assert(object.status_movement <= 1 || object.status_movement == 255); @@ -1099,14 +308,6 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); } - assert(number_of_objects2 == number_of_objects); - - for (std::size_t i = 0; i < number_of_objects; i++) { - assert(msg.objects[i] == msg2.objects[i]); - } - - assert(msg == msg2); - object_list_callback_(std::move(msg_ptr)); return true; 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 94424b5d7..68371684e 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 @@ -65,18 +65,9 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase std::mutex sensor_status_mutex_; - struct FilterStatus - { - uint8_t active; - uint8_t filter_id; - uint8_t min_value; - uint8_t max_value; - }; - - FilterStatus detection_filters_status_[DETECTION_FILTER_PROPERTIES_NUM]; - FilterStatus object_filters_status_[OBJECT_FILTER_PROPERTIES_NUM]; - - ContinentalARS548Status radar_status_; + SensorStatusPacket sensor_status_packet_{}; + FilterStatusPacket filter_status_{}; + ContinentalARS548Status radar_status_{}; std::shared_ptr parent_node_logger; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index 407632844..ec1c06190 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -52,13 +52,13 @@ struct ContinentalARS548Status std::string sensor_ip_address0; std::string sensor_ip_address1; uint8_t configuration_counter; - std::string status_longitudinal_velocity; - std::string status_longitudinal_acceleration; - std::string status_lateral_acceleration; - std::string status_yaw_rate; - std::string status_steering_angle; - std::string status_driving_direction; - std::string characteristic_speed; + std::string longitudinal_velocity_status; + std::string longitudinal_acceleration_status; + std::string lateral_acceleration_status; + std::string yaw_rate_status; + std::string steering_angle_status; + std::string driving_direction_status; + std::string characteristic_speed_status; std::string radar_status; std::string voltage_status; std::string temperature_status; @@ -122,19 +122,19 @@ struct ContinentalARS548Status os << ", "; os << "configuration_counter: " << arg.configuration_counter; os << ", "; - os << "status_longitudinal_velocity: " << arg.status_longitudinal_velocity; + os << "status_longitudinal_velocity: " << arg.longitudinal_velocity_status; os << ", "; - os << "status_longitudinal_acceleration: " << arg.status_longitudinal_acceleration; + os << "status_longitudinal_acceleration: " << arg.longitudinal_acceleration_status; os << ", "; - os << "status_lateral_acceleration: " << arg.status_lateral_acceleration; + os << "status_lateral_acceleration: " << arg.lateral_acceleration_status; os << ", "; - os << "status_yaw_rate: " << arg.status_yaw_rate; + os << "status_yaw_rate: " << arg.yaw_rate_status; os << ", "; - os << "status_steering_angle: " << arg.status_steering_angle; + os << "status_steering_angle: " << arg.steering_angle_status; os << ", "; - os << "status_driving_direction: " << arg.status_driving_direction; + os << "status_driving_direction: " << arg.driving_direction_status; os << ", "; - os << "characteristic_speed: " << arg.characteristic_speed; + os << "characteristic_speed: " << arg.characteristic_speed_status; os << ", "; os << "radar_status: " << arg.radar_status; os << ", "; 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 519724718..22fdeaa77 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 @@ -97,69 +97,47 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender( } void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) { - /*constexpr int DETECTION_LIST_METHOD_ID = 336; - constexpr int OBJECT_LIST_METHOD_ID = 329; - constexpr int SENSOR_STATUS_METHOD_ID = 380; - constexpr int FILTER_STATUS_METHOD_ID = 396; - - constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; - constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; - constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; - constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; - - constexpr int DETECTION_LIST_PDU_LENGTH = 35328; - constexpr int OBJECT_LIST_PDU_LENGTH = 9393; - constexpr int SENSOR_STATUS_PDU_LENGTH = 76; - constexpr int FILTER_STATUS_PDU_LENGTH = 322; - - */ - - if (buffer.size() < sizeof(Header)) { + if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); return; } - Header header{}; - std::memcpy(&header, buffer.data(), sizeof(Header)); + HeaderPacket header_packet{}; + std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - const uint16_t service_id = - (static_cast(buffer[SERVICE_ID_BYTE]) << 8) | buffer[SERVICE_ID_BYTE + 1]; - const uint16_t method_id = - (static_cast(buffer[METHOD_ID_BYTE]) << 8) | buffer[METHOD_ID_BYTE + 1]; - const uint32_t length = (static_cast(buffer[LENGTH_BYTE]) << 24) | - (static_cast(buffer[LENGTH_BYTE + 1]) << 16) | - (static_cast(buffer[LENGTH_BYTE + 2]) << 8) | - buffer[LENGTH_BYTE + 3]; - - assert(header.service_id.value() == service_id); - assert(header.method_id.value() == method_id); - assert(header.length.value() == length); - - if (service_id != 0) { + if (header_packet.service_id.value() != 0) { PrintError("Invalid service id"); return; - } else if (method_id == SENSOR_STATUS_METHOD_ID) { - if (buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || length != SENSOR_STATUS_PDU_LENGTH) { + } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { + if ( + buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || + header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { PrintError("SensorStatus message with invalid size"); return; } ProcessSensorStatusPacket(buffer); - } else if (method_id == FILTER_STATUS_METHOD_ID) { - if (buffer.size() != FILTER_STATUS_UDP_PAYLOAD || length != FILTER_STATUS_PDU_LENGTH) { + } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { + if ( + buffer.size() != FILTER_STATUS_UDP_PAYLOAD || + header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { PrintError("FilterStatus message with invalid size"); return; } ProcessFilterStatusPacket(buffer); - } else if (method_id == DETECTION_LIST_METHOD_ID) { - if (buffer.size() != DETECTION_LIST_UDP_PAYLOAD || length != DETECTION_LIST_PDU_LENGTH) { + } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + buffer.size() != DETECTION_LIST_UDP_PAYLOAD || + header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { PrintError("DetectionList message with invalid size"); return; } ProcessDataPacket(buffer); - } else if (method_id == OBJECT_LIST_METHOD_ID) { - if (buffer.size() != OBJECT_LIST_UDP_PAYLOAD || length != OBJECT_LIST_PDU_LENGTH) { + } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { + if ( + buffer.size() != OBJECT_LIST_UDP_PAYLOAD || + header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { PrintError("ObjectList message with invalid size"); return; } @@ -170,273 +148,161 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { - radar_status_.timestamp_nanoseconds = - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; - radar_status_.timestamp_seconds = - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 3]; - - const uint8_t & sync_status = buffer[STATUS_SYNC_STATUS_BYTE]; - - SensorStatus sensor_status{}; - std::memcpy(&sensor_status, buffer.data(), sizeof(SensorStatus)); - assert(sensor_status.stamp.timestamp_nanoseconds.value() == radar_status_.timestamp_nanoseconds); - assert(sensor_status.stamp.timestamp_seconds.value() == radar_status_.timestamp_seconds); - assert(sync_status == sensor_status.stamp.timestamp_sync_status); - - if (sync_status == 1) { + std::memcpy(&sensor_status_packet_, buffer.data(), sizeof(SensorStatusPacket)); + + radar_status_.timestamp_nanoseconds = sensor_status_packet_.stamp.timestamp_nanoseconds.value(); + radar_status_.timestamp_seconds = sensor_status_packet_.stamp.timestamp_seconds.value(); + + if (sensor_status_packet_.stamp.timestamp_sync_status == 1) { radar_status_.timestamp_sync_status = "SYNC_OK"; - } else if (sync_status == 2) { + } else if (sensor_status_packet_.stamp.timestamp_sync_status == 2) { radar_status_.timestamp_sync_status = "NEVER_SYNC"; - } else if (sync_status == 3) { + } else if (sensor_status_packet_.stamp.timestamp_sync_status == 3) { radar_status_.timestamp_sync_status = "SYNC_LOST"; } else { radar_status_.timestamp_sync_status = "INVALID_VALUE"; } - radar_status_.sw_version_major = buffer[STATUS_SW_VERSION_MAJOR_BYTE]; - radar_status_.sw_version_minor = buffer[STATUS_SW_VERSION_MINOR_BYTE]; - radar_status_.sw_version_patch = buffer[STATUS_SW_VERSION_PATCH_BYTE]; - - assert(sensor_status.sw_version_major == radar_status_.sw_version_major); - assert(sensor_status.sw_version_minor == radar_status_.sw_version_minor); - assert(sensor_status.sw_version_patch == radar_status_.sw_version_patch); - - const uint32_t status_longitudinal_u = - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 2]) << 8) | - buffer[STATUS_LONGITUDINAL_BYTE + 3]; - const uint32_t status_lateral_u = (static_cast(buffer[STATUS_LATERAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_LATERAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LATERAL_BYTE + 2]) << 8) | - buffer[STATUS_LATERAL_BYTE + 3]; - const uint32_t status_vertical_u = - (static_cast(buffer[STATUS_VERTICAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_VERTICAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_VERTICAL_BYTE + 2]) << 8) | - buffer[STATUS_VERTICAL_BYTE + 3]; - const uint32_t status_yaw_u = (static_cast(buffer[STATUS_YAW_BYTE]) << 24) | - (static_cast(buffer[STATUS_YAW_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_YAW_BYTE + 2]) << 8) | - buffer[STATUS_YAW_BYTE + 3]; - const uint32_t status_pitch_u = (static_cast(buffer[STATUS_PITCH_BYTE]) << 24) | - (static_cast(buffer[STATUS_PITCH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_PITCH_BYTE + 2]) << 8) | - buffer[STATUS_PITCH_BYTE + 3]; - const uint8_t & plug_orientation = buffer[STATUS_PLUG_ORIENTATION_BYTE]; - radar_status_.plug_orientation = plug_orientation == 0 ? "PLUG_RIGHT" - : plug_orientation == 1 ? "PLUG_LEFT" - : "INVALID_VALUE"; - const uint32_t status_length_u = (static_cast(buffer[STATUS_LENGTH_BYTE]) << 24) | - (static_cast(buffer[STATUS_LENGTH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LENGTH_BYTE + 2]) << 8) | - buffer[STATUS_LENGTH_BYTE + 3]; - const uint32_t status_width_u = (static_cast(buffer[STATUS_WIDTH_BYTE]) << 24) | - (static_cast(buffer[STATUS_WIDTH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_WIDTH_BYTE + 2]) << 8) | - buffer[STATUS_WIDTH_BYTE + 3]; - const uint32_t status_height_u = (static_cast(buffer[STATUS_HEIGHT_BYTE]) << 24) | - (static_cast(buffer[STATUS_HEIGHT_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_HEIGHT_BYTE + 2]) << 8) | - buffer[STATUS_HEIGHT_BYTE + 3]; - const uint32_t status_wheel_base_u = - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE]) << 24) | - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 2]) << 8) | - buffer[STATUS_WHEEL_BASE_BYTE + 3]; - radar_status_.max_distance = (static_cast(buffer[STATUS_MAXIMUM_DISTANCE_BYTE]) << 8) | - buffer[STATUS_MAXIMUM_DISTANCE_BYTE + 1]; - - std::memcpy(&radar_status_.longitudinal, &status_longitudinal_u, sizeof(status_longitudinal_u)); - std::memcpy(&radar_status_.lateral, &status_lateral_u, sizeof(status_lateral_u)); - std::memcpy(&radar_status_.vertical, &status_vertical_u, sizeof(status_vertical_u)); - std::memcpy(&radar_status_.yaw, &status_yaw_u, sizeof(status_yaw_u)); - - std::memcpy(&radar_status_.pitch, &status_pitch_u, sizeof(status_pitch_u)); - std::memcpy(&radar_status_.length, &status_length_u, sizeof(status_length_u)); - std::memcpy(&radar_status_.width, &status_width_u, sizeof(status_width_u)); - std::memcpy(&radar_status_.height, &status_height_u, sizeof(status_height_u)); - std::memcpy(&radar_status_.wheel_base, &status_wheel_base_u, sizeof(status_wheel_base_u)); - - const uint8_t & frequency_slot = buffer[STATUS_FREQUENCY_SLOT_BYTE]; - - if (frequency_slot == 0) { + radar_status_.sw_version_major = sensor_status_packet_.sw_version_major; + radar_status_.sw_version_minor = sensor_status_packet_.sw_version_minor; + radar_status_.sw_version_patch = sensor_status_packet_.sw_version_patch; + + radar_status_.plug_orientation = sensor_status_packet_.status.plug_orientation == 0 ? "PLUG_RIGHT" + : sensor_status_packet_.status.plug_orientation == 1 + ? "PLUG_LEFT" + : "INVALID_VALUE"; + + radar_status_.max_distance = sensor_status_packet_.status.maximum_distance.value(); + + radar_status_.longitudinal = sensor_status_packet_.status.longitudinal.value(); + radar_status_.lateral = sensor_status_packet_.status.lateral.value(); + radar_status_.vertical = sensor_status_packet_.status.vertical.value(); + radar_status_.yaw = sensor_status_packet_.status.yaw.value(); + + radar_status_.pitch = sensor_status_packet_.status.pitch.value(); + ; + radar_status_.length = sensor_status_packet_.status.length.value(); + ; + radar_status_.width = sensor_status_packet_.status.width.value(); + radar_status_.height = sensor_status_packet_.status.height.value(); + radar_status_.wheel_base = sensor_status_packet_.status.wheelbase.value(); + + if (sensor_status_packet_.status.frequency_slot == 0) { radar_status_.frequency_slot = "0:Low (76.23 GHz)"; - } else if (frequency_slot == 1) { + } else if (sensor_status_packet_.status.frequency_slot == 1) { radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; - } else if (frequency_slot == 2) { + } else if (sensor_status_packet_.status.frequency_slot == 2) { radar_status_.frequency_slot = "2:High (76.73 GHz)"; } else { radar_status_.frequency_slot = "INVALID VALUE"; } - radar_status_.cycle_time = buffer[STATUS_CYCLE_TIME_BYTE]; - radar_status_.time_slot = buffer[STATUS_TIME_SLOT_BYTE]; - - const uint8_t & hcc = buffer[STATUS_HCC_BYTE]; + radar_status_.cycle_time = sensor_status_packet_.status.cycle_time; + radar_status_.time_slot = sensor_status_packet_.status.time_slot; - radar_status_.hcc = hcc == 1 ? "Worldwide" - : hcc == 2 ? "Japan" - : ("INVALID VALUE=" + std::to_string(hcc)); + radar_status_.hcc = sensor_status_packet_.status.hcc == 1 ? "Worldwide" + : sensor_status_packet_.status.hcc == 2 + ? "Japan" + : ("INVALID VALUE=" + std::to_string(sensor_status_packet_.status.hcc)); - const uint8_t & power_save_standstill = buffer[STATUS_POWER_SAVING_STANDSTILL_BYTE]; - radar_status_.power_save_standstill = power_save_standstill == 0 ? "Off" - : power_save_standstill == 1 ? "On" - : "INVALID VALUE"; - - const uint8_t status_sensor_ip_address00 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE]; - const uint8_t status_sensor_ip_address01 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 1]; - const uint8_t status_sensor_ip_address02 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 2]; - const uint8_t status_sensor_ip_address03 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 3]; - const uint8_t status_sensor_ip_address10 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE]; - const uint8_t status_sensor_ip_address11 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 1]; - const uint8_t status_sensor_ip_address12 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 2]; - const uint8_t status_sensor_ip_address13 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 3]; + radar_status_.power_save_standstill = + sensor_status_packet_.status.powersave_standstill == 0 ? "Off" + : sensor_status_packet_.status.powersave_standstill == 1 ? "On" + : "INVALID VALUE"; std::stringstream ss0, ss1; - ss0 << std::to_string(status_sensor_ip_address00) << "." - << std::to_string(status_sensor_ip_address01) << "." - << std::to_string(status_sensor_ip_address02) << "." - << std::to_string(status_sensor_ip_address03); + ss0 << std::to_string(sensor_status_packet_.status.sensor_ip_address00) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address01) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address02) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address03); radar_status_.sensor_ip_address0 = ss0.str(); - ss1 << std::to_string(status_sensor_ip_address10) << "." - << std::to_string(status_sensor_ip_address11) << "." - << std::to_string(status_sensor_ip_address12) << "." - << std::to_string(status_sensor_ip_address13); + ss1 << std::to_string(sensor_status_packet_.status.sensor_ip_address10) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address11) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address12) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address13); radar_status_.sensor_ip_address1 = ss1.str(); - assert(sensor_status.status.plug_orientation == plug_orientation); - assert(sensor_status.status.maximum_distance.value() == radar_status_.max_distance); - assert(sensor_status.status.longitudinal.value() == radar_status_.longitudinal); - assert(sensor_status.status.lateral.value() == radar_status_.lateral); - assert(sensor_status.status.vertical.value() == radar_status_.vertical); - assert(sensor_status.status.yaw.value() == radar_status_.yaw); - assert(sensor_status.status.pitch.value() == radar_status_.pitch); - assert(sensor_status.status.length.value() == radar_status_.length); - assert(sensor_status.status.width.value() == radar_status_.width); - assert(sensor_status.status.height.value() == radar_status_.height); - assert(sensor_status.status.wheelbase.value() == radar_status_.wheel_base); - assert(sensor_status.status.frequency_slot == frequency_slot); - assert(sensor_status.status.cycle_time == radar_status_.cycle_time); - assert(sensor_status.status.time_slot == radar_status_.time_slot); - assert(sensor_status.status.hcc == hcc); - assert(sensor_status.status.powersave_standstill == power_save_standstill); - assert(sensor_status.status.sensor_ip_address00 == status_sensor_ip_address00); - assert(sensor_status.status.sensor_ip_address01 == status_sensor_ip_address01); - assert(sensor_status.status.sensor_ip_address02 == status_sensor_ip_address02); - assert(sensor_status.status.sensor_ip_address03 == status_sensor_ip_address03); - - assert(sensor_status.status.sensor_ip_address10 == status_sensor_ip_address10); - assert(sensor_status.status.sensor_ip_address11 == status_sensor_ip_address11); - assert(sensor_status.status.sensor_ip_address12 == status_sensor_ip_address12); - assert(sensor_status.status.sensor_ip_address13 == status_sensor_ip_address13); - - radar_status_.configuration_counter = buffer[STATUS_CONFIGURATION_COUNTER_BYTE]; - assert(sensor_status.configuration_counter == radar_status_.configuration_counter); - - const uint8_t & status_longitudinal_velocity = buffer[STATUS_LONGITUDINAL_VELOCITY_BYTE]; - assert(sensor_status.status_longitudinal_velocity == status_longitudinal_velocity); - radar_status_.status_longitudinal_velocity = status_longitudinal_velocity == 0 ? "VDY_OK" - : status_longitudinal_velocity == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_longitudinal_acceleration = buffer[STATUS_LONGITUDINAL_ACCELERATION_BYTE]; - assert(sensor_status.status_longitudinal_acceleration == status_longitudinal_acceleration); - radar_status_.status_longitudinal_acceleration = status_longitudinal_acceleration == 0 ? "VDY_OK" - : status_longitudinal_acceleration == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_lateral_acceleration = buffer[STATUS_LATERAL_ACCELERATION_BYTE]; - assert(sensor_status.status_lateral_acceleration == status_lateral_acceleration); - radar_status_.status_lateral_acceleration = status_lateral_acceleration == 0 ? "VDY_OK" - : status_lateral_acceleration == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_yaw_rate = buffer[STATUS_YAW_RATE_BYTE]; - assert(sensor_status.status_yaw_rate == status_yaw_rate); - radar_status_.status_yaw_rate = status_yaw_rate == 0 ? "VDY_OK" - : status_yaw_rate == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_steering_angle = buffer[STATUS_STEERING_ANGLE_BYTE]; - assert(sensor_status.status_steering_angle == status_steering_angle); - radar_status_.status_steering_angle = status_steering_angle == 0 ? "VDY_OK" - : status_steering_angle == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_driving_direction = buffer[STATUS_DRIVING_DIRECTION_BYTE]; - assert(sensor_status.status_driving_direction == status_driving_direction); - radar_status_.status_driving_direction = status_driving_direction == 0 ? "VDY_OK" - : status_driving_direction == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & characteristic_speed = buffer[STATUS_CHARACTERISTIC_SPEED_BYTE]; - assert(sensor_status.status_characteristic_speed == characteristic_speed); - radar_status_.characteristic_speed = characteristic_speed == 0 ? "VDY_OK" - : characteristic_speed == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & radar_status = buffer[STATUS_RADAR_STATUS_BYTE]; - assert(sensor_status.status_radar_status == radar_status); - if (radar_status == 0) { + radar_status_.configuration_counter = sensor_status_packet_.configuration_counter; + + radar_status_.longitudinal_velocity_status = + sensor_status_packet_.longitudinal_velocity_status == 0 ? "VDY_OK" + : sensor_status_packet_.longitudinal_velocity_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.longitudinal_acceleration_status = + sensor_status_packet_.longitudinal_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet_.longitudinal_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.lateral_acceleration_status = + sensor_status_packet_.lateral_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet_.lateral_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.yaw_rate_status = sensor_status_packet_.yaw_rate_status == 0 ? "VDY_OK" + : sensor_status_packet_.yaw_rate_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.steering_angle_status = sensor_status_packet_.steering_angle_status == 0 ? "VDY_OK" + : sensor_status_packet_.steering_angle_status == 1 + ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.driving_direction_status = + sensor_status_packet_.driving_direction_status == 0 ? "VDY_OK" + : sensor_status_packet_.driving_direction_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.characteristic_speed_status = + sensor_status_packet_.characteristic_speed_status == 0 ? "VDY_OK" + : sensor_status_packet_.characteristic_speed_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + if (sensor_status_packet_.radar_status == 0) { radar_status_.radar_status = "STATE_INIT"; - } else if (radar_status == 1) { + } else if (sensor_status_packet_.radar_status == 1) { radar_status_.radar_status = "STATE_OK"; - } else if (radar_status == 2) { + } else if (sensor_status_packet_.radar_status == 2) { radar_status_.radar_status = "STATE_INVALID"; } else { radar_status_.radar_status = "INVALID VALUE"; } - const uint8_t & voltage_status = buffer[STATUS_VOLTAGE_STATUS_BYTE]; - assert(sensor_status.status_voltage_status == voltage_status); - if (voltage_status == 0) { + if (sensor_status_packet_.voltage_status == 0) { radar_status_.voltage_status = "Ok"; } - if (voltage_status & 0x01) { + if (sensor_status_packet_.voltage_status & 0x01) { radar_status_.voltage_status += "Current under voltage"; } - if (voltage_status & 0x02) { + if (sensor_status_packet_.voltage_status & 0x02) { radar_status_.voltage_status = "Past under voltage"; } - if (voltage_status & 0x03) { + if (sensor_status_packet_.voltage_status & 0x03) { radar_status_.voltage_status = "Current over voltage"; } - if (voltage_status & 0x04) { + if (sensor_status_packet_.voltage_status & 0x04) { radar_status_.voltage_status = "Past over voltage"; } - const uint8_t & temperature_status = buffer[STATUS_TEMPERATURE_STATUS_BYTE]; - assert(sensor_status.status_temperature_status == temperature_status); - if (temperature_status == 0) { + if (sensor_status_packet_.temperature_status == 0) { radar_status_.temperature_status = "Ok"; } - if (temperature_status & 0x01) { + if (sensor_status_packet_.temperature_status & 0x01) { radar_status_.temperature_status += "Current under temperature"; } - if (temperature_status & 0x02) { + if (sensor_status_packet_.temperature_status & 0x02) { radar_status_.temperature_status += "Past under temperature"; } - if (temperature_status & 0x03) { + if (sensor_status_packet_.temperature_status & 0x03) { radar_status_.temperature_status += "Current over temperature"; } - if (temperature_status & 0x04) { + if (sensor_status_packet_.temperature_status & 0x04) { radar_status_.temperature_status += "Past over temperature"; } - const uint8_t & blockage_status = buffer[STATUS_BLOCKAGE_STATUS_BYTE]; - assert(sensor_status.status_blockage_status == blockage_status); - const uint8_t & blockage_status0 = blockage_status & 0x0f; - const uint8_t & blockage_status1 = (blockage_status & 0xf0) >> 4; + const uint8_t & blockage_status0 = sensor_status_packet_.blockage_status & 0x0f; + const uint8_t & blockage_status1 = (sensor_status_packet_.blockage_status & 0xf0) >> 4; if (blockage_status0 == 0) { radar_status_.blockage_status = "Blind"; @@ -465,73 +331,8 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { - // assert(false); // it seems 548 does not have this anymore.... - // Unused available data - // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; - // constexpr int FILTER_STATUS_TIMESTAMP_SECONDS_BYTE = 12; - // constexpr int FILTER_STATUS_SYNC_STATUS_BYTE = 16; - // constexpr int FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE = 17; - // constexpr int FILTER_STATUS_DETECTION_SORT_INDEX_BYTE = 18; - // constexpr int FILTER_STATUS_OBJECT_SORT_INDEX_BYTE = 19; - constexpr int FILTER_STATUS_DETECTION_FILTER_BYTE = 20; - constexpr int FILTER_STATUS_OBJECT_FILTER_BYTE = 90; - - // Unused available data - // const uint32_t filter_status_timestamp_nanoseconds = - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - // buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; const uint32_t - // filter_status_timestamp_seconds = - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - // buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 3]; const uint8_t filter_status_sync_status = - // buffer[FILTER_STATUS_SYNC_STATUS_BYTE]; const uint8_t - // filter_status_filter_configuration_counter = - // buffer[FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE]; const uint8_t - // filter_status_detection_sort_index = buffer[FILTER_STATUS_DETECTION_SORT_INDEX_BYTE]; const - // uint8_t filter_status_object_sort_index = buffer[FILTER_STATUS_OBJECT_SORT_INDEX_BYTE]; - - std::size_t byte_index = FILTER_STATUS_DETECTION_FILTER_BYTE; - for (int property_index = 1; property_index <= 7; property_index++) { - FilterStatus filter_status; - filter_status.active = buffer[byte_index++]; - assert(buffer[byte_index] == property_index); - filter_status.filter_id = buffer[byte_index++]; - filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - - detection_filters_status_[property_index - 1] = filter_status; - } - - byte_index = FILTER_STATUS_OBJECT_FILTER_BYTE; - for (int property_index = 1; property_index <= 24; property_index++) { - FilterStatus filter_status; - filter_status.active = buffer[byte_index++]; - assert(buffer[byte_index] == property_index); - filter_status.filter_id = buffer[byte_index++]; - filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - - object_filters_status_[property_index - 1] = filter_status; - } + assert(buffer.size() == sizeof(FilterStatusPacket)); + std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); } void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) @@ -572,14 +373,6 @@ Status ContinentalARS548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) { - constexpr int CONFIGURATION_LONGITUDINAL_BYTE = 8; - constexpr int CONFIGURATION_LATERAL_BYTE = 12; - constexpr int CONFIGURATION_VERTICAL_BYTE = 16; - constexpr int CONFIGURATION_YAW_BYTE = 20; - constexpr int CONFIGURATION_PITCH_BYTE = 24; - constexpr int CONFIGURATION_PLUG_ORIENTATION_BYTE = 28; - constexpr int CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE = 60; - if ( longitudinal_autosar < -100.f || longitudinal_autosar > 100.f || lateral_autosar < -100.f || lateral_autosar > 100.f || vertical_autosar < 0.01f || vertical_autosar > 10.f || @@ -589,69 +382,20 @@ Status ContinentalARS548HwInterface::SetSensorMounting( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - uint8_t longitudinal_autosar_bytes[4]; - uint8_t lateral_autosar_bytes[4]; - uint8_t vertical_autosar_bytes[4]; - uint8_t yaw_autosar_bytes[4]; - uint8_t pitch_autosar_bytes[4]; - std::memcpy(longitudinal_autosar_bytes, &longitudinal_autosar, sizeof(longitudinal_autosar)); - std::memcpy(lateral_autosar_bytes, &lateral_autosar, sizeof(lateral_autosar)); - std::memcpy(vertical_autosar_bytes, &vertical_autosar, sizeof(vertical_autosar)); - std::memcpy(yaw_autosar_bytes, &yaw_autosar, sizeof(yaw_autosar)); - std::memcpy(pitch_autosar_bytes, &pitch_autosar, sizeof(pitch_autosar)); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 0] = longitudinal_autosar_bytes[3]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 1] = longitudinal_autosar_bytes[2]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 2] = longitudinal_autosar_bytes[1]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 3] = longitudinal_autosar_bytes[0]; - - send_vector[CONFIGURATION_LATERAL_BYTE + 0] = lateral_autosar_bytes[3]; - send_vector[CONFIGURATION_LATERAL_BYTE + 1] = lateral_autosar_bytes[2]; - send_vector[CONFIGURATION_LATERAL_BYTE + 2] = lateral_autosar_bytes[1]; - send_vector[CONFIGURATION_LATERAL_BYTE + 3] = lateral_autosar_bytes[0]; - - send_vector[CONFIGURATION_VERTICAL_BYTE + 0] = vertical_autosar_bytes[3]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 1] = vertical_autosar_bytes[2]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 2] = vertical_autosar_bytes[1]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 3] = vertical_autosar_bytes[0]; - - send_vector[CONFIGURATION_YAW_BYTE + 0] = yaw_autosar_bytes[3]; - send_vector[CONFIGURATION_YAW_BYTE + 1] = yaw_autosar_bytes[2]; - send_vector[CONFIGURATION_YAW_BYTE + 2] = yaw_autosar_bytes[1]; - send_vector[CONFIGURATION_YAW_BYTE + 3] = yaw_autosar_bytes[0]; - - send_vector[CONFIGURATION_PITCH_BYTE + 0] = pitch_autosar_bytes[3]; - send_vector[CONFIGURATION_PITCH_BYTE + 1] = pitch_autosar_bytes[2]; - send_vector[CONFIGURATION_PITCH_BYTE + 2] = pitch_autosar_bytes[1]; - send_vector[CONFIGURATION_PITCH_BYTE + 3] = pitch_autosar_bytes[0]; - - send_vector[CONFIGURATION_PLUG_ORIENTATION_BYTE] = plug_orientation; - send_vector[CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.longitudinal = longitudinal_autosar; - configuration.configuration.lateral = lateral_autosar; - configuration.configuration.vertical = vertical_autosar; - configuration.configuration.yaw = yaw_autosar; - configuration.configuration.pitch = pitch_autosar; - configuration.configuration.plug_orientation = plug_orientation; - configuration.new_sensor_mounting = 1; - - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + ConfigurationPacket configuration_packet{}; + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.longitudinal = longitudinal_autosar; + configuration_packet.configuration.lateral = lateral_autosar; + configuration_packet.configuration.vertical = vertical_autosar; + configuration_packet.configuration.yaw = yaw_autosar; + configuration_packet.configuration.pitch = pitch_autosar; + configuration_packet.configuration.plug_orientation = plug_orientation; + configuration_packet.new_sensor_mounting = 1; + + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -661,12 +405,6 @@ Status ContinentalARS548HwInterface::SetSensorMounting( Status ContinentalARS548HwInterface::SetVehicleParameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) { - constexpr int CONFIGURATION_LENGTH_BYTE = 29; - constexpr int CONFIGURATION_WIDTH_BYTE = 33; - constexpr int CONFIGURATION_HEIGHT_BYTE = 37; - constexpr int CONFIGURATION_WHEEL_BASE_BYTE = 41; - constexpr int CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE = 61; - if ( length_autosar < 0.01f || length_autosar > 100.f || width_autosar < 0.01f || width_autosar > 100.f || height_autosar < 0.01f || height_autosar > 100.f || @@ -675,59 +413,19 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - uint8_t length_autosar_bytes[4]; - uint8_t width_autosar_bytes[4]; - uint8_t height_autosar_bytes[4]; - uint8_t wheel_base_autosar_bytes[4]; - std::memcpy(length_autosar_bytes, &length_autosar, sizeof(length_autosar)); - std::memcpy(width_autosar_bytes, &width_autosar, sizeof(width_autosar)); - std::memcpy(height_autosar_bytes, &height_autosar, sizeof(height_autosar)); - std::memcpy(wheel_base_autosar_bytes, &wheel_base_autosar, sizeof(wheel_base_autosar)); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_LENGTH_BYTE + 0] = length_autosar_bytes[3]; - send_vector[CONFIGURATION_LENGTH_BYTE + 1] = length_autosar_bytes[2]; - send_vector[CONFIGURATION_LENGTH_BYTE + 2] = length_autosar_bytes[1]; - send_vector[CONFIGURATION_LENGTH_BYTE + 3] = length_autosar_bytes[0]; - - send_vector[CONFIGURATION_WIDTH_BYTE + 0] = width_autosar_bytes[3]; - send_vector[CONFIGURATION_WIDTH_BYTE + 1] = width_autosar_bytes[2]; - send_vector[CONFIGURATION_WIDTH_BYTE + 2] = width_autosar_bytes[1]; - send_vector[CONFIGURATION_WIDTH_BYTE + 3] = width_autosar_bytes[0]; - - send_vector[CONFIGURATION_HEIGHT_BYTE + 0] = height_autosar_bytes[3]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 1] = height_autosar_bytes[2]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 2] = height_autosar_bytes[1]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 3] = height_autosar_bytes[0]; - - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 0] = wheel_base_autosar_bytes[3]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 1] = wheel_base_autosar_bytes[2]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 2] = wheel_base_autosar_bytes[1]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 3] = wheel_base_autosar_bytes[0]; - - send_vector[CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.length = length_autosar; - configuration.configuration.width = width_autosar; - configuration.configuration.height = height_autosar; - configuration.configuration.wheelbase = wheel_base_autosar; - configuration.new_vehicle_parameters = 1; + ConfigurationPacket configuration_packet{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.length = length_autosar; + configuration_packet.configuration.width = width_autosar; + configuration_packet.configuration.height = height_autosar; + configuration_packet.configuration.wheelbase = wheel_base_autosar; + configuration_packet.new_vehicle_parameters = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -738,14 +436,6 @@ Status ContinentalARS548HwInterface::SetRadarParameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t power_save_standstill) { - constexpr int CONFIGURATION_MAXIMUM_DISTANCE_BYTE = 45; - constexpr int CONFIGURATION_FREQUENCY_SLOT_BYTE = 47; - constexpr int CONFIGURATION_CYCLE_TIME_BYTE = 48; - constexpr int CONFIGURATION_TIME_SLOT_BYTE = 49; - constexpr int CONFIGURATION_HCC_BYTE = 50; - constexpr int CONFIGURATION_power_save_STANDSTILL_BYTE = 51; - constexpr int CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE = 62; - if ( maximum_distance < 93 || maximum_distance > 1514 || frequency_slot > 2 || cycle_time < 50 || cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 || @@ -754,42 +444,20 @@ Status ContinentalARS548HwInterface::SetRadarParameters( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 0] = - static_cast((maximum_distance >> 8) & 0xff); - send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 1] = - static_cast(maximum_distance & 0xff); - - send_vector[CONFIGURATION_FREQUENCY_SLOT_BYTE] = frequency_slot; - send_vector[CONFIGURATION_CYCLE_TIME_BYTE] = cycle_time; - send_vector[CONFIGURATION_TIME_SLOT_BYTE] = time_slot; - send_vector[CONFIGURATION_HCC_BYTE] = hcc; - send_vector[CONFIGURATION_power_save_STANDSTILL_BYTE] = power_save_standstill; - - send_vector[CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.maximum_distance = maximum_distance; - configuration.configuration.frequency_slot = frequency_slot; - configuration.configuration.cycle_time = cycle_time; - configuration.configuration.hcc = hcc; - configuration.configuration.powersave_standstill = power_save_standstill; - configuration.new_radar_parameters = 1; + ConfigurationPacket configuration_packet{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.maximum_distance = maximum_distance; + configuration_packet.configuration.frequency_slot = frequency_slot; + configuration_packet.configuration.cycle_time = cycle_time; + configuration_packet.configuration.hcc = hcc; + configuration_packet.configuration.powersave_standstill = power_save_standstill; + configuration_packet.new_radar_parameters = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -798,10 +466,6 @@ Status ContinentalARS548HwInterface::SetRadarParameters( Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) { - constexpr int CONFIGURATION_SENSOR_IP_ADDRESS0 = 52; - constexpr int CONFIGURATION_SENSOR_IP_ADDRESS1 = 56; - constexpr int CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE = 63; - std::array ip_bytes; try { @@ -812,29 +476,8 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 0] = ip_bytes[0]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 1] = ip_bytes[1]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 2] = ip_bytes[2]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 3] = ip_bytes[3]; - - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 0] = 169; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 1] = 254; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 2] = 116; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 3] = 113; - - send_vector[CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); + ConfigurationPacket configuration{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); configuration.header.service_id = CONFIGURATION_SERVICE_ID; configuration.header.method_id = CONFIGURATION_METHOD_ID; configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; @@ -848,9 +491,8 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens configuration.configuration.sensor_ip_address13 = 113; configuration.new_network_configuration = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -862,36 +504,22 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_PAYLOAD_SIZE = ACCELERATION_LATERAL_COG_LENGTH + 8; + const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { PrintError("Invalid lateral_acceleration value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &lateral_acceleration, sizeof(lateral_acceleration)); - - std::vector send_vector(ACCELERATION_LATERAL_COG_PAYLOAD_SIZE, 0); - send_vector[1] = ACCELERATION_LATERAL_COG_SERVICE_ID; - send_vector[2] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID >> 8); - send_vector[3] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID & 0x00ff); - send_vector[7] = ACCELERATION_LATERAL_COG_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - AccelerationLateralCoG acceleration_lateral_cog{}; - assert(send_vector.size() == sizeof(AccelerationLateralCoG)); + AccelerationLateralCoGPacket acceleration_lateral_cog{}; + static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; - std::vector send_vector2(sizeof(AccelerationLateralCoG)); - std::memcpy(send_vector2.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoG)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); + std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -903,37 +531,24 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE = ACCELERATION_LONGITUDINAL_COG_LENGTH + 8; + const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { PrintError("Invalid longitudinal_acceleration value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &longitudinal_acceleration, sizeof(longitudinal_acceleration)); - - std::vector send_vector(ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE, 0); - send_vector[1] = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - send_vector[2] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID >> 8); - send_vector[3] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID & 0x00ff); - send_vector[7] = ACCELERATION_LONGITUDINAL_COG_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - AccelerationLongitudinalCoG acceleration_longitudinal_cog{}; - assert(send_vector.size() == sizeof(AccelerationLongitudinalCoG)); + AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; + static_assert( + sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; - std::vector send_vector2(sizeof(AccelerationLongitudinalCoG)); + std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); std::memcpy( - send_vector2.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoG)); - assert(send_vector2 == send_vector2); + send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -945,30 +560,22 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_PAYLOAD_SIZE = CHARACTERISTIC_SPEED_LENGTH + 8; + const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; if (characteristic_speed < 0.f || characteristic_speed > 255.f) { PrintError("Invalid characteristic_speed value"); return Status::ERROR_1; } - std::vector send_vector(CHARACTERISTIC_SPEED_PAYLOAD_SIZE, 0); - send_vector[1] = CHARACTERISTIC_SPEED_SERVICE_ID; - send_vector[2] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID >> 8); - send_vector[3] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID & 0x00ff); - send_vector[7] = CHARACTERISTIC_SPEED_LENGTH; - send_vector[10] = static_cast(characteristic_speed); - - CharasteristicSpeed characteristic_speed_packet{}; - assert(send_vector.size() == sizeof(CharasteristicSpeed)); + CharasteristicSpeedPacket characteristic_speed_packet{}; + static_assert(sizeof(CharasteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; characteristic_speed_packet.characteristic_speed = characteristic_speed; - std::vector send_vector2(sizeof(CharasteristicSpeed)); - std::memcpy(send_vector2.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeed)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(CharasteristicSpeedPacket)); + std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeedPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -983,24 +590,10 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_PAYLOAD_SIZE = DRIVING_DIRECTION_LENGTH + 8; - - std::vector send_vector(DRIVING_DIRECTION_PAYLOAD_SIZE, 0); - send_vector[1] = DRIVING_DIRECTION_SERVICE_ID; - send_vector[2] = static_cast(DRIVING_DIRECTION_METHOD_ID >> 8); - send_vector[3] = static_cast(DRIVING_DIRECTION_METHOD_ID & 0xff); - send_vector[7] = DRIVING_DIRECTION_LENGTH; + const int DRIVING_DIRECTION_UDP_LENGTH = 30; - if (direction == 0) { - send_vector[9] = DRIVING_DIRECTION_STANDSTILL; - } else if (direction > 0) { - send_vector[9] = DRIVING_DIRECTION_FORWARD; - } else { - send_vector[9] = DRIVING_DIRECTION_BACKWARDS; - } - - DrivingDirection driving_direction_packet{}; - assert(send_vector.size() == sizeof(DrivingDirection)); + DrivingDirectionPacket driving_direction_packet{}; + static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; @@ -1013,9 +606,8 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; } - std::vector send_vector2(sizeof(DrivingDirection)); - std::memcpy(send_vector2.data(), &driving_direction_packet, sizeof(DrivingDirection)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(DrivingDirectionPacket)); + std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1027,37 +619,23 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_PAYLOAD_SIZE = STEERING_ANGLE_LENGTH + 8; + const int STEERING_ANGLE_UDP_LENGTH = 40; if (angle_rad < -M_PI_2 || angle_rad > M_PI_2) { PrintError("Invalid angle_rad value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &angle_rad, sizeof(angle_rad)); - - std::vector send_vector(STEERING_ANGLE_PAYLOAD_SIZE, 0); - send_vector[1] = STEERING_ANGLE_SERVICE_ID; - send_vector[2] = static_cast(STEERING_ANGLE_METHOD_ID >> 8); - send_vector[3] = static_cast(STEERING_ANGLE_METHOD_ID & 0x00ff); - send_vector[7] = STEERING_ANGLE_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - SteeringAngleFrontAxle steering_angle_front_axle_packet{}; - assert(send_vector.size() == sizeof(SteeringAngleFrontAxle)); + SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; - std::vector send_vector2(sizeof(SteeringAngleFrontAxle)); + std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); std::memcpy( - send_vector2.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxle)); - assert(send_vector2 == send_vector2); + send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1069,31 +647,17 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_PAYLOAD_SIZE = VELOCITY_VEHICLE_LENGTH + 8; - - uint8_t bytes[4]; - std::memcpy(bytes, &velocity, sizeof(velocity)); - - std::vector send_vector(VELOCITY_VEHICLE_PAYLOAD_SIZE, 0); - send_vector[1] = VELOCITY_VEHICLE_SERVICE_ID; - send_vector[2] = static_cast(VELOCITY_VEHICLE_METHOD_ID >> 8); - send_vector[3] = static_cast(VELOCITY_VEHICLE_METHOD_ID & 0x00ff); - send_vector[7] = VELOCITY_VEHICLE_LENGTH; - send_vector[11] = bytes[3]; - send_vector[12] = bytes[2]; - send_vector[13] = bytes[1]; - send_vector[14] = bytes[0]; - - VelocityVehicle steering_angle_front_axle_packet{}; - assert(send_vector.size() == sizeof(VelocityVehicle)); + const int VELOCITY_VEHICLE_UDP_SIZE = 36; + + VelocityVehiclePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; steering_angle_front_axle_packet.velocity_vehicle = velocity; - std::vector send_vector2(sizeof(VelocityVehicle)); - std::memcpy(send_vector2.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehicle)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(VelocityVehiclePacket)); + std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1105,31 +669,17 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) constexpr uint16_t YAW_RATE_SERVICE_ID = 0; constexpr uint16_t YAW_RATE_METHOD_ID = 326; constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_PAYLOAD_SIZE = YAW_RATE_LENGTH + 8; - - uint8_t bytes[4]; - std::memcpy(bytes, &yaw_rate, sizeof(yaw_rate)); - - std::vector send_vector(YAW_RATE_PAYLOAD_SIZE, 0); - send_vector[1] = YAW_RATE_SERVICE_ID; - send_vector[2] = static_cast(YAW_RATE_METHOD_ID >> 8); - send_vector[3] = static_cast(YAW_RATE_METHOD_ID & 0x00ff); - send_vector[7] = YAW_RATE_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - YawRate yaw_rate_packet{}; - assert(send_vector.size() == sizeof(YawRate)); + const int YAW_RATE_UDP_SIZE = 40; + + YawRatePacket yaw_rate_packet{}; + static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; yaw_rate_packet.header.length = YAW_RATE_LENGTH; yaw_rate_packet.yaw_rate = yaw_rate; - std::vector send_vector2(sizeof(YawRate)); - std::memcpy(send_vector2.data(), &yaw_rate_packet, sizeof(YawRate)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(YawRatePacket)); + std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 1234abd06..33aee3622 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -308,14 +308,14 @@ void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostics.add("sensor_ip_address0", sensor_status.sensor_ip_address0); diagnostics.add("sensor_ip_address1", sensor_status.sensor_ip_address1); diagnostics.add("configuration_counter", std::to_string(sensor_status.configuration_counter)); - diagnostics.add("status_longitudinal_velocity", sensor_status.status_longitudinal_velocity); + diagnostics.add("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); diagnostics.add( - "status_longitudinal_acceleration", sensor_status.status_longitudinal_acceleration); - diagnostics.add("status_lateral_acceleration", sensor_status.status_lateral_acceleration); - diagnostics.add("status_yaw_rate", sensor_status.status_yaw_rate); - diagnostics.add("status_steering_angle", sensor_status.status_steering_angle); - diagnostics.add("status_driving_direction", sensor_status.status_driving_direction); - diagnostics.add("characteristic_speed", sensor_status.characteristic_speed); + "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); + diagnostics.add("lateral_acceleration_status", sensor_status.lateral_acceleration_status); + diagnostics.add("yaw_rate_status", sensor_status.yaw_rate_status); + diagnostics.add("steering_angle_status", sensor_status.steering_angle_status); + diagnostics.add("driving_direction_status", sensor_status.driving_direction_status); + diagnostics.add("characteristic_speed_status", sensor_status.characteristic_speed_status); diagnostics.add("radar_status", sensor_status.radar_status); diagnostics.add("voltage_status", sensor_status.voltage_status); diagnostics.add("temperature_status", sensor_status.temperature_status); From 4ce4874cc4dd04bdc0d486b597b7d7f71e235fb9 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 20 Dec 2023 17:48:50 +0900 Subject: [PATCH 15/42] chore: mixed misspells Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 16 ++++++++-------- .../decoders/continental_ars548_decoder.cpp | 2 +- .../continental_ars548_hw_interface.cpp | 8 ++++---- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index a9de08fa1..5686d8d8b 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -71,7 +71,7 @@ struct HeaderPacket big_uint32_buf_t length; }; -struct HeaderSOMEIPPacket +struct HeaderSomeIPPacket { big_uint16_buf_t client_id; big_uint16_buf_t session_id; @@ -120,7 +120,7 @@ struct DetectionPacket struct DetectionListPacket { HeaderPacket header; - HeaderSOMEIPPacket header_someip; + HeaderSomeIPPacket header_some_ip; HeaderE2EP07Packet header_e2ep07; StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; @@ -146,7 +146,7 @@ struct DetectionListPacket big_float32_buf_t alignment_azimuth_correction; big_float32_buf_t alignment_elevation_correction; uint8_t alignment_status; - uint8_t reserverd[14]; + uint8_t reserved[14]; }; struct ObjectPacket @@ -167,9 +167,9 @@ struct ObjectPacket big_float32_buf_t position_covariance_xy; big_float32_buf_t position_orientation; big_float32_buf_t position_orientation_std; - uint8_t existance_invalid_flags; - big_float32_buf_t existance_probability; - big_float32_buf_t existance_ppv; + uint8_t existence_invalid_flags; + big_float32_buf_t existence_probability; + big_float32_buf_t existence_ppv; uint8_t classification_car; uint8_t classification_truck; uint8_t classification_motorcycle; @@ -220,7 +220,7 @@ struct ObjectPacket struct ObjectListPacket { HeaderPacket header; - HeaderSOMEIPPacket header_someip; + HeaderSomeIPPacket header_some_ip; HeaderE2EP07Packet header_e2ep07; StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; @@ -305,7 +305,7 @@ struct AccelerationLongitudinalCoGPacket uint8_t reserved1[22]; }; -struct CharasteristicSpeedPacket +struct CharacteristicSpeedPacket { HeaderPacket header; uint8_t reserved0[2]; 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 c9e46a30d..70e1e7d02 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 @@ -264,7 +264,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector object_msg.orientation = object.position_orientation.value(); object_msg.orientation_std = object.position_orientation_std.value(); - object_msg.existence_probability = object.existance_probability.value(); + object_msg.existence_probability = object.existence_probability.value(); object_msg.classification_car = object.classification_car; object_msg.classification_truck = object.classification_truck; object_msg.classification_motorcycle = object.classification_motorcycle; 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 22fdeaa77..596d7b5a8 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 @@ -567,15 +567,15 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic return Status::ERROR_1; } - CharasteristicSpeedPacket characteristic_speed_packet{}; - static_assert(sizeof(CharasteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); + CharacteristicSpeedPacket characteristic_speed_packet{}; + static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; characteristic_speed_packet.characteristic_speed = characteristic_speed; - std::vector send_vector(sizeof(CharasteristicSpeedPacket)); - std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeedPacket)); + std::vector send_vector(sizeof(CharacteristicSpeedPacket)); + std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); From 2338af727c305890391d6daa356c55319e204dc5 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 20 Dec 2023 18:41:13 +0900 Subject: [PATCH 16/42] fix: fixed short circuit in conditional statement, missing lock, and typo Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_types.hpp | 2 +- .../continental_ars548_hw_interface.cpp | 2 ++ .../continental_ars548_hw_interface_ros_wrapper.cpp | 10 ++++------ 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index ec1c06190..ccdb917cf 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -142,7 +142,7 @@ struct ContinentalARS548Status os << ", "; os << "voltage_status: " << arg.voltage_status; os << ", "; - os << "blockage_status_rate: " << arg.blockage_status; + os << "blockage_status: " << arg.blockage_status; return os; } 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 596d7b5a8..6b46d3d02 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 @@ -148,6 +148,8 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { + std::lock_guard l(sensor_status_mutex_); + std::memcpy(&sensor_status_packet_, buffer.data(), sizeof(SensorStatusPacket)); radar_status_.timestamp_nanoseconds = sensor_status_packet_.stamp.timestamp_nanoseconds.value(); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 33aee3622..805879fef 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -209,12 +209,10 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: std::string sensor_model_str; std::string return_mode_str; if ( - get_param(p, "sensor_model", sensor_model_str) || - get_param(p, "return_mode", return_mode_str) || get_param(p, "host_ip", new_param.host_ip) || - get_param(p, "sensor_ip", new_param.sensor_ip) || - get_param(p, "frame_id", new_param.frame_id) || - get_param(p, "data_port", new_param.data_port) || - get_param(p, "configuration_host_port", new_param.configuration_host_port) || + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | + get_param(p, "host_ip", new_param.host_ip) | get_param(p, "sensor_ip", new_param.sensor_ip) | + get_param(p, "frame_id", new_param.frame_id) | get_param(p, "data_port", new_param.data_port) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { if (0 < sensor_model_str.length()) new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); From 8a0f9441b8b16a932d766c077a7533739380cbda Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 24 Jan 2024 11:23:59 +0900 Subject: [PATCH 17/42] feat: added use_receive_time options, enriched the PointCloud2 messages for evaluation, and implemented some ROS logic to set the radar configuration and dynamic input Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 73 ++++- .../continental/continental_common.hpp | 30 +- .../decoders/continental_ars548_decoder.hpp | 17 +- .../decoders/continental_packets_decoder.hpp | 2 +- .../decoders/continental_ars548_decoder.cpp | 35 ++- .../continental_ars548_hw_interface.hpp | 2 +- .../continental_types.hpp | 2 +- .../continental_ars548_hw_interface.cpp | 24 +- ...continental_ars548_decoder_ros_wrapper.hpp | 40 +-- ...nental_ars548_hw_interface_ros_wrapper.hpp | 69 +++-- .../launch/continental_launch_all_hw.xml | 58 +++- nebula_ros/package.xml | 4 + ...continental_ars548_decoder_ros_wrapper.cpp | 193 +++++++++++- ...nental_ars548_hw_interface_ros_wrapper.cpp | 287 ++++++++++++++++-- 14 files changed, 738 insertions(+), 98 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 5686d8d8b..38ad0c4f4 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -366,6 +366,77 @@ struct FilterStatusPacket #pragma pack(pop) +struct PointARS548Detection +{ + PCL_ADD_POINT4D; + float azimuth; + float azimuth_std; + float elevation; + float elevation_std; + float range; + float range_std; + int8_t rcs; + uint16_t measurement_id; + uint8_t positive_predictive_value; + uint8_t classification; + uint8_t multi_target_probability; + uint16_t object_id; + uint8_t ambiguity_flag; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + +// Note we only use a subset of the data since POINT_CLOUD_REGISTER_POINT_STRUCT has a limit in the +// number of fields +struct PointARS548Object +{ + PCL_ADD_POINT4D; + uint32_t id; + uint16_t age; + uint8_t status_measurement; + uint8_t status_movement; + uint8_t position_reference; + uint8_t classification_car; + uint8_t classification_truck; + uint8_t classification_motorcycle; + uint8_t classification_bicycle; + uint8_t classification_pedestrian; + float dynamics_abs_vel_x; + float dynamics_abs_vel_y; + float dynamics_rel_vel_x; + float dynamics_rel_vel_y; + float shape_length_edge_mean; + float shape_width_edge_mean; + float dynamics_orientation_rate_mean; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +} EIGEN_ALIGN16; + } // namespace continental_ars548 } // namespace drivers } // namespace nebula + +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_ars548::PointARS548Detection, + (float, x, x)(float, y, y)(float, z, z)(float, azimuth, azimuth)(float, azimuth_std, azimuth_std)( + float, elevation, elevation)(float, elevation_std, elevation_std)(float, range, range)( + float, range_std, range_std)(int8_t, rcs, rcs)(uint16_t, measurement_id, measurement_id)( + uint8_t, positive_predictive_value, + positive_predictive_value)(uint8_t, classification, classification)( + uint8_t, multi_target_probability, multi_target_probability)(uint16_t, object_id, object_id)( + uint8_t, ambiguity_flag, ambiguity_flag)) + +// Note: we can only use up to 20 fields +POINT_CLOUD_REGISTER_POINT_STRUCT( + nebula::drivers::continental_ars548::PointARS548Object, + (float, x, x)(float, y, y)(float, z, z)(uint32_t, id, id)(uint16_t, age, age)( + uint8_t, status_measurement, status_measurement)(uint8_t, status_movement, status_movement)( + uint8_t, position_reference, + position_reference)(uint8_t, classification_car, classification_car)( + uint8_t, classification_truck, + classification_truck)(uint8_t, classification_motorcycle, classification_motorcycle)( + uint8_t, classification_bicycle, + classification_bicycle)(uint8_t, classification_pedestrian, classification_pedestrian)( + float, dynamics_abs_vel_x, dynamics_abs_vel_x)(float, dynamics_abs_vel_y, dynamics_abs_vel_y)( + float, dynamics_rel_vel_x, dynamics_rel_vel_x)(float, dynamics_rel_vel_y, dynamics_rel_vel_y)( + float, shape_length_edge_mean, + shape_length_edge_mean)(float, shape_width_edge_mean, shape_width_edge_mean)( + float, dynamics_orientation_rate_mean, dynamics_orientation_rate_mean)) diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp index 078f7fe0d..f3f081705 100644 --- a/nebula_common/include/nebula_common/continental/continental_common.hpp +++ b/nebula_common/include/nebula_common/continental/continental_common.hpp @@ -1,5 +1,5 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -33,8 +33,21 @@ namespace drivers struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; + std::string new_sensor_ip{}; uint16_t configuration_host_port{}; uint16_t configuration_sensor_port{}; + bool use_sensor_time{}; + uint16_t new_plug_orientation{}; + float new_vehicle_length{}; + float new_vehicle_width{}; + float new_vehicle_height{}; + float new_vehicle_wheelbase{}; + uint16_t new_radar_maximum_distance{}; + uint16_t new_radar_frequency_slot{}; + uint16_t new_radar_cycle_time{}; + uint16_t new_radar_time_slot{}; + uint16_t new_radar_country_code{}; + uint16_t new_radar_powersave_standstill{}; }; /// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << @@ -46,8 +59,21 @@ inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip + << ", NewSensorIP: " << arg.new_sensor_ip << ", ConfigurationHostPort: " << arg.configuration_host_port - << ", ConfigurationSensorPort: " << arg.configuration_sensor_port; + << ", ConfigurationSensorPort: " << arg.configuration_sensor_port + << ", UseSensorTime: " << arg.use_sensor_time + << ", NewPlugOrientation: " << arg.new_plug_orientation + << ", NewVehicleLength: " << arg.new_vehicle_length + << ", NewVehicleWidth: " << arg.new_vehicle_width + << ", NewVehicleHeight: " << arg.new_vehicle_height + << ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase + << ", NewRadarMaximumDistance: " << arg.new_radar_maximum_distance + << ", NewRadarFrequencySlot: " << arg.new_radar_frequency_slot + << ", NewRadarCycleTime: " << arg.new_radar_cycle_time + << ", NewRadarTimeSlot: " << arg.new_radar_time_slot + << ", NewRadarCountryCode: " << arg.new_radar_country_code + << ", RadarPowersaveStandstill: " << arg.new_radar_powersave_standstill; return os; } 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 80e622017..fe8abcecc 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,13 +14,14 @@ #pragma once -#include "nebula_common/continental/continental_common.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp" +#include +#include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" #include #include +#include +#include +#include #include #include @@ -46,8 +47,10 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @return Resulting flag bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override; - bool ParseDetectionsListPacket(const std::vector & data); - bool ParseObjectsListPacket(const std::vector & data); + bool ParseDetectionsListPacket( + const std::vector & data, const std_msgs::msg::Header & header); + bool ParseObjectsListPacket( + const std::vector & data, const std_msgs::msg::Header & header); /// @brief Register function to call whenever a new detection list is processed /// @param detection_list_callback 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 7342e2b0b..9efca35b8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 70e1e7d02..a399a7de1 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -75,19 +75,20 @@ bool ContinentalARS548Decoder::ProcessPackets( return false; } - return ParseDetectionsListPacket(data); + return ParseDetectionsListPacket(data, nebula_packets.header); } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } - return ParseObjectsListPacket(data); + return ParseObjectsListPacket(data, nebula_packets.header); } return true; } -bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector & data) +bool ContinentalARS548Decoder::ParseDetectionsListPacket( + const std::vector & data, const std_msgs::msg::Header & header) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; @@ -98,8 +99,14 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vectorframe_id; - msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); - msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); + + if (sensor_configuration_->use_sensor_time) { + msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); + } else { + msg.header.stamp = header.stamp; + } + msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); @@ -186,7 +193,8 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector & data) +bool ContinentalARS548Decoder::ParseObjectsListPacket( + const std::vector & data, const std_msgs::msg::Header & header) { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; @@ -196,9 +204,16 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector std::memcpy(&object_list, data.data(), sizeof(object_list)); - msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); - msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); + // msg.header.frame_id = sensor_configuration_->frame_id; + msg.header.frame_id = "base_link"; + + if (sensor_configuration_->use_sensor_time) { + msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); + } else { + msg.header.stamp = header.stamp; + } + msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); 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 68371684e..cccbd0c69 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index ccdb917cf..7a7485f75 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nebula_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 6b46d3d02..3fdb59280 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,7 +18,7 @@ #include "nebula_common/continental/continental_ars548.hpp" #include - +#include namespace nebula { namespace drivers @@ -399,6 +399,13 @@ Status ContinentalARS548HwInterface::SetSensorMounting( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); + 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("yaw_autosar = " + std::to_string(yaw_autosar)); + PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); + PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -429,6 +436,11 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); + PrintInfo("length_autosar = " + std::to_string(length_autosar)); + PrintInfo("width_autosar = " + std::to_string(width_autosar)); + PrintInfo("height_autosar = " + std::to_string(height_autosar)); + PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -461,6 +473,12 @@ Status ContinentalARS548HwInterface::SetRadarParameters( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); + PrintInfo("maximum_distance = " + std::to_string(maximum_distance)); + PrintInfo("frequency_slot = " + std::to_string(frequency_slot)); + PrintInfo("cycle_time = " + std::to_string(cycle_time)); + PrintInfo("hcc = " + std::to_string(hcc)); + PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -478,6 +496,8 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens return Status::SENSOR_CONFIG_ERROR; } + PrintInfo("New sensor IP = " + sensor_ip_address); + ConfigurationPacket configuration{}; static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); configuration.header.service_id = CONFIGURATION_SERVICE_ID; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp index 1f9fcc4ec..4e16ff1b6 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,26 +15,26 @@ #ifndef NEBULA_ContinentalARS548DriverRosWrapper_H #define NEBULA_ContinentalARS548DriverRosWrapper_H -#include "nebula_common/continental/continental_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" -#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" -#include "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" - #include #include +#include +#include +#include +#include +#include +#include +#include #include #include -#include "continental_msgs/msg/continental_ars548_detection.hpp" -#include "continental_msgs/msg/continental_ars548_detection_list.hpp" -#include "continental_msgs/msg/continental_ars548_object.hpp" -#include "continental_msgs/msg/continental_ars548_object_list.hpp" -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" -#include "radar_msgs/msg/radar_scan.hpp" -#include "radar_msgs/msg/radar_tracks.hpp" +#include +#include +#include +#include +#include +#include +#include +#include #include #include @@ -56,6 +56,8 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive rclcpp::Publisher::SharedPtr object_list_pub_; rclcpp::Publisher::SharedPtr object_pointcloud_pub_; rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; + rclcpp::Publisher::SharedPtr scan_raw_pub_; + rclcpp::Publisher::SharedPtr objects_raw_pub_; std::shared_ptr sensor_cfg_ptr_; @@ -106,13 +108,13 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @brief Convert ARS548 detections to a pointcloud /// @param msg The ARS548 detection list msg /// @return Resulting detection pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalArs548DetectionList & msg); + pcl::PointCloud::Ptr + ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); /// @brief Convert ARS548 objects to a pointcloud /// @param msg The ARS548 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( + pcl::PointCloud::Ptr ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert ARS548 detections to a standard RadarScan msg diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp index f95882685..a7f42b87c 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,21 +15,22 @@ #ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H #define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H -#include "boost_tcp_driver/tcp_driver.hpp" -#include "nebula_common/continental/continental_common.hpp" -#include "nebula_common/nebula_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp" -#include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" - #include +#include #include +#include +#include +#include +#include #include #include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_srvs/srv/empty.hpp" +#include +#include +#include +#include +#include +#include #include @@ -73,8 +74,15 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, /// @brief Received Continental Radar message publisher rclcpp::Publisher::SharedPtr packets_pub_; - rclcpp::Subscription::SharedPtr driving_direction_sub_; - rclcpp::Service::SharedPtr sensor_config_service_server_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr set_new_sensor_ip_service_server_; + rclcpp::Service::SharedPtr set_new_sensor_mounting_service_server_; + rclcpp::Service::SharedPtr set_new_vehicle_parameters_service_server_; + rclcpp::Service::SharedPtr set_new_radar_parameters_service_server_; /// @brief Initializing hardware interface ros wrapper /// @param sensor_configuration SensorConfiguration for this driver @@ -85,14 +93,39 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, /// @param packets_buffer Received NebulaPackets void ReceivePacketsDataCallback(std::unique_ptr packets_buffer); - /// @brief (Dummy) Callback to set the driving direction - /// @param msg A dummy msg - void DrivingDirectionCallback(const std_msgs::msg::Bool::SharedPtr msg); + /// @brief Callback to send the odometry information to the radar device + /// @param msg The odometry message + void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the acceleration information to the radar device + /// @param msg The acceleration message + void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + + /// @brief Service callback to set the new sensor ip + /// @param request Empty service request + /// @param response Empty service response + void SetNewSensorIPRequestCallback( + const std::shared_ptr request, + const std::shared_ptr response); + + /// @brief Service callback to set the new sensor mounting position + /// @param request Empty service request + /// @param response Empty service response + void SetNewSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr response); + + /// @brief Service callback to set the new vehicle parameters + /// @param request Empty service request + /// @param response Empty service response + void SetNewVehicleParametersRequestCallback( + const std::shared_ptr request, + const std::shared_ptr response); - /// @brief (Dummy) Service callback to configure the sensor + /// @brief Service callback to set the new radar parameters /// @param request Empty service request /// @param response Empty service response - void SensorConfigureRequestCallback( + void SetNewRadarParametersRequestCallback( const std::shared_ptr request, const std::shared_ptr response); diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 6d487803b..126340217 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -3,30 +3,80 @@ - + + + + + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index dc2b1c51a..2cc202e64 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -15,6 +15,7 @@ continental_msgs diagnostic_msgs diagnostic_updater + geometry_msgs libpcl-all-dev nebula_common nebula_decoders @@ -24,6 +25,9 @@ rclcpp_components robosense_msgs std_srvs + tf2_eigen + tf2_geometry_msgs + tf2_ros velodyne_msgs yaml-cpp diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index caa0c28dc..c662d80aa 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -51,14 +51,20 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( detection_list_pub_ = this->create_publisher( - "detection_array", rclcpp::SensorDataQoS()); + "continental_detections", rclcpp::SensorDataQoS()); object_list_pub_ = this->create_publisher( - "object_array", rclcpp::SensorDataQoS()); + "continental_objects", rclcpp::SensorDataQoS()); detection_pointcloud_pub_ = this->create_publisher( "detection_points", rclcpp::SensorDataQoS()); object_pointcloud_pub_ = this->create_publisher("object_points", rclcpp::SensorDataQoS()); + + scan_raw_pub_ = + this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); + ; + objects_raw_pub_ = + this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); } void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( @@ -135,6 +141,124 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( this->declare_parameter("frame_id", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_plug_orientation", descriptor); + sensor_configuration.new_plug_orientation = + this->get_parameter("new_plug_orientation").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_length", descriptor); + sensor_configuration.new_vehicle_length = + static_cast(this->get_parameter("new_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_width", descriptor); + sensor_configuration.new_vehicle_width = + static_cast(this->get_parameter("new_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_height", descriptor); + sensor_configuration.new_vehicle_height = + static_cast(this->get_parameter("new_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_wheelbase", descriptor); + sensor_configuration.new_vehicle_wheelbase = + static_cast(this->get_parameter("new_vehicle_wheelbase").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_maximum_distance", descriptor); + sensor_configuration.new_radar_maximum_distance = + this->get_parameter("new_radar_maximum_distance").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_frequency_slot", descriptor); + sensor_configuration.new_radar_frequency_slot = + this->get_parameter("new_radar_frequency_slot").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_cycle_time", descriptor); + sensor_configuration.new_radar_cycle_time = + this->get_parameter("new_radar_cycle_time").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_time_slot", descriptor); + sensor_configuration.new_radar_time_slot = this->get_parameter("new_radar_time_slot").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_country_code", descriptor); + sensor_configuration.new_radar_country_code = + this->get_parameter("new_radar_country_code").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_powersave_standstill", descriptor); + sensor_configuration.new_radar_powersave_standstill = + this->get_parameter("new_radar_powersave_standstill").as_int(); + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -163,6 +287,14 @@ void ContinentalARS548DriverRosWrapper::DetectionListCallback( detection_pointcloud_msg_ptr->header = msg->header; detection_pointcloud_pub_->publish(std::move(detection_pointcloud_msg_ptr)); } + + if ( + scan_raw_pub_->get_subscription_count() > 0 || + scan_raw_pub_->get_intra_process_subscription_count() > 0) { + auto radar_scan_msg = ConvertToRadarScan(*msg); + radar_scan_msg.header = msg->header; + scan_raw_pub_->publish(std::move(radar_scan_msg)); + } if ( detection_list_pub_->get_subscription_count() > 0 || detection_list_pub_->get_intra_process_subscription_count() > 0) { @@ -183,6 +315,13 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( 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 ( object_list_pub_->get_subscription_count() > 0 || object_list_pub_->get_intra_process_subscription_count() > 0) { @@ -190,13 +329,15 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } -pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalARS548DriverRosWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { - pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.detections.size()); - pcl::PointXYZ point{}; + nebula::drivers::continental_ars548::PointARS548Detection point{}; for (const auto & detection : msg.detections) { point.x = std::cos(detection.elevation_angle) * std::cos(detection.azimuth_angle) * detection.range; @@ -204,6 +345,20 @@ pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertTo std::cos(detection.elevation_angle) * std::sin(detection.azimuth_angle) * detection.range; point.z = std::sin(detection.elevation_angle) * detection.range; + point.azimuth = detection.azimuth_angle; + point.azimuth_std = detection.azimuth_angle_std; + point.elevation = detection.elevation_angle; + point.elevation_std = detection.elevation_angle_std; + point.range = detection.range; + point.range_std = detection.range_std; + point.rcs = detection.rcs; + point.measurement_id = detection.measurement_id; + point.positive_predictive_value = detection.positive_predictive_value; + point.classification = detection.classification; + point.multi_target_probability = detection.multi_target_probability; + point.object_id = detection.object_id; + point.ambiguity_flag = detection.ambiguity_flag; + output_pointcloud->points.emplace_back(point); } @@ -212,18 +367,38 @@ pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertTo return output_pointcloud; } -pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertToPointcloud( +pcl::PointCloud::Ptr +ContinentalARS548DriverRosWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { - pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); output_pointcloud->reserve(msg.objects.size()); - pcl::PointXYZ point{}; + nebula::drivers::continental_ars548::PointARS548Object point{}; for (const auto & detection : msg.objects) { point.x = static_cast(detection.position.x); point.y = static_cast(detection.position.y); point.z = static_cast(detection.position.z); + point.id = detection.object_id; + point.age = detection.age; + point.status_measurement = detection.status_measurement; + point.status_movement = detection.status_movement; + point.position_reference = detection.position_reference; + point.classification_car = detection.classification_car; + point.classification_truck = detection.classification_truck; + point.classification_motorcycle = detection.classification_motorcycle; + point.classification_bicycle = detection.classification_bicycle; + point.classification_pedestrian = detection.classification_pedestrian; + point.dynamics_abs_vel_x = static_cast(detection.absolute_velocity.x); + point.dynamics_abs_vel_y = static_cast(detection.absolute_velocity.y); + point.dynamics_rel_vel_x = static_cast(detection.relative_velocity.x); + point.dynamics_rel_vel_y = static_cast(detection.relative_velocity.y); + point.shape_length_edge_mean = detection.shape_length_edge_mean; + point.shape_width_edge_mean = detection.shape_width_edge_mean; + point.dynamics_orientation_rate_mean = detection.orientation_rate_mean; + output_pointcloud->points.emplace_back(point); } diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 805879fef..afa3d85b0 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2024 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp" +#include + +#include +#include #include #include @@ -50,16 +53,38 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - driving_direction_sub_ = create_subscription( - "driving_direction", rclcpp::SensorDataQoS(), + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, std::bind( - &ContinentalARS548HwInterfaceRosWrapper::DrivingDirectionCallback, this, - std::placeholders::_1)); + &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, std::placeholders::_1)); - sensor_config_service_server_ = this->create_service( - "configure_radar", std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SensorConfigureRequestCallback, - this, std::placeholders::_1, std::placeholders::_2)); + set_new_sensor_ip_service_server_ = this->create_service( + "set_new_sensor_ip", std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); + + set_new_sensor_mounting_service_server_ = this->create_service( + "set_new_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_new_vehicle_parameters_service_server_ = this->create_service( + "set_new_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_new_radar_parameters_service_server_ = this->create_service( + "set_new_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); StreamStart(); } @@ -72,8 +97,11 @@ Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { interface_status_ = hw_interface_.CloudInterfaceStart(); + + std::scoped_lock lock(mtx_config_); diagnostics_updater_.add( - "radar_status", this, &ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus); + "radar_status(" + sensor_configuration_.frame_id + ")", this, + &ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus); } return interface_status_; @@ -129,6 +157,15 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("sensor_ip", descriptor); sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_sensor_ip", descriptor); + sensor_configuration.new_sensor_ip = this->get_parameter("new_sensor_ip").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; @@ -176,6 +213,124 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( sensor_configuration.configuration_sensor_port = this->get_parameter("configuration_sensor_port").as_int(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_plug_orientation", descriptor); + sensor_configuration.new_plug_orientation = + this->get_parameter("new_plug_orientation").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_length", descriptor); + sensor_configuration.new_vehicle_length = + static_cast(this->get_parameter("new_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_width", descriptor); + sensor_configuration.new_vehicle_width = + static_cast(this->get_parameter("new_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_height", descriptor); + sensor_configuration.new_vehicle_height = + static_cast(this->get_parameter("new_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_vehicle_wheelbase", descriptor); + sensor_configuration.new_vehicle_wheelbase = + static_cast(this->get_parameter("new_vehicle_wheelbase").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_maximum_distance", descriptor); + sensor_configuration.new_radar_maximum_distance = + this->get_parameter("new_radar_maximum_distance").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_frequency_slot", descriptor); + sensor_configuration.new_radar_frequency_slot = + this->get_parameter("new_radar_frequency_slot").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_cycle_time", descriptor); + sensor_configuration.new_radar_cycle_time = + this->get_parameter("new_radar_cycle_time").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_time_slot", descriptor); + sensor_configuration.new_radar_time_slot = this->get_parameter("new_radar_time_slot").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_country_code", descriptor); + sensor_configuration.new_radar_country_code = + this->get_parameter("new_radar_country_code").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("new_radar_powersave_standstill", descriptor); + sensor_configuration.new_radar_powersave_standstill = + this->get_parameter("new_radar_powersave_standstill").as_int(); + } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -211,6 +366,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: if ( get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | get_param(p, "host_ip", new_param.host_ip) | get_param(p, "sensor_ip", new_param.sensor_ip) | + get_param(p, "new_sensor_ip", new_param.new_sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | get_param(p, "data_port", new_param.data_port) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { @@ -236,24 +392,108 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: return *result; } -void ContinentalARS548HwInterfaceRosWrapper::DrivingDirectionCallback( - [[maybe_unused]] const std_msgs::msg::Bool::SharedPtr msg) +void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { - hw_interface_.SetDrivingDirection(0); - hw_interface_.SetAccelerationLateralCog(0.0); - hw_interface_.SetAccelerationLongitudinalCog(0.0); - - hw_interface_.SetCharacteristicSpeed(0.0); - hw_interface_.SetSteeringAngleFrontAxle(0.0); - hw_interface_.SetVelocityVehicle(5.0); - hw_interface_.SetYawRate(0.25); + std::scoped_lock lock(mtx_config_); + // hw_interface_.SetCharacteristicSpeed(0.0); + // hw_interface_.SetSteeringAngleFrontAxle(0.0); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * msg->twist.twist.linear.x); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); } -void ContinentalARS548HwInterfaceRosWrapper::SensorConfigureRequestCallback( +void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback( [[maybe_unused]] const std::shared_ptr request, [[maybe_unused]] const std::shared_ptr response) { - hw_interface_.SetVehicleParameters(10.f, 4.5f, 3.f, 2.5f); + std::scoped_lock lock(mtx_config_); + hw_interface_.SetSensorIPAddress(sensor_configuration_.new_sensor_ip); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback( + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) +{ + std::scoped_lock lock(mtx_config_); + + auto tf_buffer = std::make_unique(this->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + geometry_msgs::msg::TransformStamped autosar_to_sensor_tf; + try { + autosar_to_sensor_tf = tf_buffer->lookupTransform( + std::string("autosar"), sensor_configuration_.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not obtain the transform from the autosar frame to %s (%s)", + sensor_configuration_.frame_id.c_str(), ex.what()); + return; + } + + const auto & quat = autosar_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); + + std::cout << "DEBUG TF" << std::endl; + std::cout << "translation: x=" << autosar_to_sensor_tf.transform.translation.x + << " y=" << autosar_to_sensor_tf.transform.translation.y + << " z=" << autosar_to_sensor_tf.transform.translation.z << std::endl; + std::cout << "orientation: roll=" << rpy.x << " pitch=" << rpy.y << " yaw=" << rpy.z << std::endl; + + hw_interface_.SetSensorMounting( + autosar_to_sensor_tf.transform.translation.x, autosar_to_sensor_tf.transform.translation.y, + autosar_to_sensor_tf.transform.translation.z, rpy.z, rpy.y, + sensor_configuration_.new_plug_orientation); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetVehicleParameters( + sensor_configuration_.new_vehicle_length, sensor_configuration_.new_vehicle_width, + sensor_configuration_.new_vehicle_height, sensor_configuration_.new_vehicle_wheelbase); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] const std::shared_ptr response) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetRadarParameters( + sensor_configuration_.new_radar_maximum_distance, + sensor_configuration_.new_radar_frequency_slot, sensor_configuration_.new_radar_cycle_time, + sensor_configuration_.new_radar_time_slot, sensor_configuration_.new_radar_country_code, + sensor_configuration_.new_radar_powersave_standstill); } std::vector @@ -268,6 +508,7 @@ ContinentalARS548HwInterfaceRosWrapper::updateParameters() {rclcpp::Parameter("sensor_model", os_sensor_model.str()), rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), + rclcpp::Parameter("new_sensor_ip", sensor_configuration_.new_sensor_ip), rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), rclcpp::Parameter("data_port", sensor_configuration_.data_port), rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), From 2506fb7ec8c080db9e4dfc5b7c7c50eda58b794b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 24 Jan 2024 13:35:38 +0900 Subject: [PATCH 18/42] feat: added the missing steerin angle input Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental_ars548_hw_interface_ros_wrapper.hpp | 6 ++++++ nebula_ros/launch/continental_launch_all_hw.xml | 2 ++ .../continental_ars548_hw_interface_ros_wrapper.cpp | 12 ++++++++++++ 3 files changed, 20 insertions(+) diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp index a7f42b87c..ed2ed3964 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include #include @@ -76,6 +77,7 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, rclcpp::Publisher::SharedPtr packets_pub_; rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; bool standstill_{true}; @@ -101,6 +103,10 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, /// @param msg The acceleration message void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + /// @brief Callback to send the steering angle information to the radar device + /// @param msg The steering angle message + void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + /// @brief Service callback to set the new sensor ip /// @param request Empty service request /// @param response Empty service response diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 126340217..a0f6caff0 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -5,6 +5,7 @@ + @@ -54,6 +55,7 @@ + diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index afa3d85b0..0d7da394b 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -63,6 +63,11 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( std::bind( &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, std::placeholders::_1)); + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + set_new_sensor_ip_service_server_ = this->create_service( "set_new_sensor_ip", std::bind( &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback, @@ -429,6 +434,13 @@ void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); } +void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetSteeringAngleFrontAxle(msg->data); +} + void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback( [[maybe_unused]] const std::shared_ptr request, [[maybe_unused]] const std::shared_ptr response) From 871b78ac716256932bc23492b946a1bacd57ad59 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 1 Feb 2024 20:43:24 +0900 Subject: [PATCH 19/42] feat: missing features for proper vehicle integration. fixed fixes and added visualization markers Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 2 + .../continental/continental_common.hpp | 3 +- .../decoders/continental_ars548_decoder.cpp | 5 +- ...continental_ars548_decoder_ros_wrapper.hpp | 25 +- .../launch/continental_launch_all_hw.xml | 3 + nebula_ros/package.xml | 1 + ...continental_ars548_decoder_ros_wrapper.cpp | 339 ++++++++++++++---- ...nental_ars548_hw_interface_ros_wrapper.cpp | 111 +++--- 8 files changed, 367 insertions(+), 122 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 38ad0c4f4..d2d3246b6 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -375,6 +375,8 @@ struct PointARS548Detection float elevation_std; float range; float range_std; + float range_rate; + float range_rate_std; int8_t rcs; uint16_t measurement_id; uint8_t positive_predictive_value; diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp index f3f081705..ea4d5e949 100644 --- a/nebula_common/include/nebula_common/continental/continental_common.hpp +++ b/nebula_common/include/nebula_common/continental/continental_common.hpp @@ -34,6 +34,7 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; std::string new_sensor_ip{}; + std::string base_frame{}; uint16_t configuration_host_port{}; uint16_t configuration_sensor_port{}; bool use_sensor_time{}; @@ -59,7 +60,7 @@ inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", NewSensorIP: " << arg.new_sensor_ip + << ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame << ", ConfigurationHostPort: " << arg.configuration_host_port << ", ConfigurationSensorPort: " << arg.configuration_sensor_port << ", UseSensorTime: " << arg.use_sensor_time 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 a399a7de1..a63436979 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 @@ -204,8 +204,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( std::memcpy(&object_list, data.data(), sizeof(object_list)); - // msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.frame_id = "base_link"; + msg.header.frame_id = sensor_configuration_->base_frame; if (sensor_configuration_->use_sensor_time) { msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); @@ -268,7 +267,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( object_msg.position.x = static_cast(object.position_x.value()); object_msg.position.y = static_cast(object.position_y.value()); - object_msg.position.y = static_cast(object.position_z.value()); + object_msg.position.z = static_cast(object.position_z.value()); object_msg.position_std.x = static_cast(object.position_x_std.value()); object_msg.position_std.y = static_cast(object.position_y_std.value()); diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp index 4e16ff1b6..e3636dd0f 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp @@ -35,9 +35,11 @@ #include #include #include +#include #include #include +#include namespace nebula { @@ -58,8 +60,23 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive rclcpp::Publisher::SharedPtr detection_pointcloud_pub_; rclcpp::Publisher::SharedPtr scan_raw_pub_; rclcpp::Publisher::SharedPtr objects_raw_pub_; + rclcpp::Publisher::SharedPtr objects_markers_pub_; - std::shared_ptr sensor_cfg_ptr_; + std::unordered_set previous_ids_; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { + {{{-1.0, -1.0}}, + {{-1.0, 0.0}}, + {{-1.0, 1.0}}, + {{0.0, 1.0}}, + {{1.0, 1.0}}, + {{1.0, 0.0}}, + {{1.0, -1.0}}, + {{0.0, -1.0}}, + {{0.0, 0.0}}}}; + + std::shared_ptr sensor_cfg_ptr_; drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; @@ -128,6 +145,12 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @return Resulting RadarTracks msg radar_msgs::msg::RadarTracks ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg); + + /// @brief Convert ARS548 objects to a standard MarkerArray msg + /// @param msg The ARS548 object list msg + /// @return Resulting MarkerArray msg + visualization_msgs::msg::MarkerArray ConvertToMarkers( + const continental_msgs::msg::ContinentalArs548ObjectList & msg); }; } // namespace ros diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index a0f6caff0..fb603d137 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -2,6 +2,7 @@ + @@ -31,6 +32,7 @@ + @@ -59,6 +61,7 @@ + diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 2cc202e64..5e4d6c208 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -29,6 +29,7 @@ tf2_geometry_msgs tf2_ros velodyne_msgs + visualization_msgs yaml-cpp ament_cmake_gtest diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index c662d80aa..4f9efea8c 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp" +#include -#include "pcl_conversions/pcl_conversions.h" +#include namespace nebula { @@ -40,8 +40,8 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( sensor_cfg_ptr_ = std::make_shared(sensor_configuration); - wrapper_status_ = - InitializeDriver(std::const_pointer_cast(sensor_cfg_ptr_)); + wrapper_status_ = InitializeDriver( + std::const_pointer_cast(sensor_cfg_ptr_)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); packets_sub_ = create_subscription( @@ -62,9 +62,12 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( scan_raw_pub_ = this->create_publisher("scan_raw", rclcpp::SensorDataQoS()); - ; + objects_raw_pub_ = this->create_publisher("objects_raw", rclcpp::SensorDataQoS()); + + objects_markers_pub_ = + this->create_publisher("marker_array", 10); } void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( @@ -141,6 +144,15 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( this->declare_parameter("frame_id", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -295,6 +307,7 @@ void ContinentalARS548DriverRosWrapper::DetectionListCallback( radar_scan_msg.header = msg->header; scan_raw_pub_->publish(std::move(radar_scan_msg)); } + if ( detection_list_pub_->get_subscription_count() > 0 || detection_list_pub_->get_intra_process_subscription_count() > 0) { @@ -315,6 +328,7 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( 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) { @@ -322,6 +336,14 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( 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) { @@ -351,6 +373,8 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( point.elevation_std = detection.elevation_angle_std; point.range = detection.range; point.range_std = detection.range_std; + point.range_rate = detection.range_rate; + point.range_rate_std = detection.range_rate_std; point.rcs = detection.rcs; point.measurement_id = detection.measurement_id; point.positive_predictive_value = detection.positive_predictive_value; @@ -376,28 +400,39 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( output_pointcloud->reserve(msg.objects.size()); nebula::drivers::continental_ars548::PointARS548Object point{}; - for (const auto & detection : msg.objects) { - point.x = static_cast(detection.position.x); - point.y = static_cast(detection.position.y); - point.z = static_cast(detection.position.z); - - point.id = detection.object_id; - point.age = detection.age; - point.status_measurement = detection.status_measurement; - point.status_movement = detection.status_movement; - point.position_reference = detection.position_reference; - point.classification_car = detection.classification_car; - point.classification_truck = detection.classification_truck; - point.classification_motorcycle = detection.classification_motorcycle; - point.classification_bicycle = detection.classification_bicycle; - point.classification_pedestrian = detection.classification_pedestrian; - point.dynamics_abs_vel_x = static_cast(detection.absolute_velocity.x); - point.dynamics_abs_vel_y = static_cast(detection.absolute_velocity.y); - point.dynamics_rel_vel_x = static_cast(detection.relative_velocity.x); - point.dynamics_rel_vel_y = static_cast(detection.relative_velocity.y); - point.shape_length_edge_mean = detection.shape_length_edge_mean; - point.shape_width_edge_mean = detection.shape_width_edge_mean; - point.dynamics_orientation_rate_mean = detection.orientation_rate_mean; + for (const auto & object : msg.objects) { + const double half_length = 0.5 * object.shape_length_edge_mean; + const double half_width = 0.5 * object.shape_width_edge_mean; + const int reference_index = std::min(object.position_reference, 8); + const double & yaw = object.orientation; + const double x = object.position.x + + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + const double y = object.position.y + + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + + point.x = static_cast(x); + point.y = static_cast(y); + point.z = static_cast(object.position.z); + + point.id = object.object_id; + point.age = object.age; + point.status_measurement = object.status_measurement; + point.status_movement = object.status_movement; + point.position_reference = object.position_reference; + point.classification_car = object.classification_car; + point.classification_truck = object.classification_truck; + point.classification_motorcycle = object.classification_motorcycle; + point.classification_bicycle = object.classification_bicycle; + point.classification_pedestrian = object.classification_pedestrian; + point.dynamics_abs_vel_x = static_cast(object.absolute_velocity.x); + point.dynamics_abs_vel_y = static_cast(object.absolute_velocity.y); + point.dynamics_rel_vel_x = static_cast(object.relative_velocity.x); + point.dynamics_rel_vel_y = static_cast(object.relative_velocity.y); + point.shape_length_edge_mean = object.shape_length_edge_mean; + point.shape_width_edge_mean = object.shape_width_edge_mean; + point.dynamics_orientation_rate_mean = object.orientation_rate_mean; output_pointcloud->points.emplace_back(point); } @@ -407,7 +442,7 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::ConvertToRadarScan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -433,7 +468,7 @@ radar_msgs::msg::RadarScan ConvertToRadarScan( return output_msg; } -radar_msgs::msg::RadarTracks ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; @@ -446,67 +481,72 @@ radar_msgs::msg::RadarTracks ConvertToRadarTracks( constexpr int16_t MOTORCYCLE_ID = 32005; constexpr int16_t BICYCLE_ID = 32006; constexpr int16_t PEDESTRIAN_ID = 32007; + constexpr float INVALID_COVARIANCE = 1e6; radar_msgs::msg::RadarTrack track_msg; - for (const auto & detection : msg.objects) { - track_msg.uuid.uuid[0] = static_cast(detection.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((detection.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((detection.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((detection.object_id >> 24) & 0xff); - track_msg.position = detection.position; - track_msg.velocity = detection.absolute_velocity; - track_msg.acceleration = detection.absolute_acceleration; - track_msg.size.x = detection.shape_length_edge_mean; - track_msg.size.y = detection.shape_width_edge_mean; + for (const auto & objects : msg.objects) { + track_msg.uuid.uuid[0] = static_cast(objects.object_id & 0xff); + track_msg.uuid.uuid[1] = static_cast((objects.object_id >> 8) & 0xff); + track_msg.uuid.uuid[2] = static_cast((objects.object_id >> 16) & 0xff); + track_msg.uuid.uuid[3] = static_cast((objects.object_id >> 24) & 0xff); + track_msg.position = objects.position; + track_msg.velocity = objects.absolute_velocity; + track_msg.acceleration = objects.absolute_acceleration; + track_msg.size.x = objects.shape_length_edge_mean; + track_msg.size.y = objects.shape_width_edge_mean; track_msg.size.z = 1.f; - uint8_t max_score = detection.classification_unknown; + uint8_t max_score = objects.classification_unknown; track_msg.classification = UNKNOWN_ID; - if (detection.classification_car > max_score) { - max_score = detection.classification_car; + if (objects.classification_car > max_score) { + max_score = objects.classification_car; track_msg.classification = CAR_ID; } - if (detection.classification_truck > max_score) { - max_score = detection.classification_truck; + if (objects.classification_truck > max_score) { + max_score = objects.classification_truck; track_msg.classification = TRUCK_ID; } - if (detection.classification_motorcycle > max_score) { - max_score = detection.classification_motorcycle; + if (objects.classification_motorcycle > max_score) { + max_score = objects.classification_motorcycle; track_msg.classification = MOTORCYCLE_ID; } - if (detection.classification_bicycle > max_score) { - max_score = detection.classification_bicycle; + if (objects.classification_bicycle > max_score) { + max_score = objects.classification_bicycle; track_msg.classification = BICYCLE_ID; } - if (detection.classification_pedestrian > max_score) { - max_score = detection.classification_pedestrian; + if (objects.classification_pedestrian > max_score) { + max_score = objects.classification_pedestrian; track_msg.classification = PEDESTRIAN_ID; } - track_msg.position_covariance[0] = static_cast(detection.position_std.x); - track_msg.position_covariance[1] = detection.position_covariance_xy; + track_msg.position_covariance[0] = static_cast(objects.position_std.x); + track_msg.position_covariance[1] = objects.position_covariance_xy; track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = static_cast(detection.position_std.y); + track_msg.position_covariance[3] = static_cast(objects.position_std.y); track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = static_cast(detection.position_std.z); + track_msg.position_covariance[5] = static_cast(objects.position_std.z); - track_msg.velocity_covariance[0] = static_cast(detection.absolute_velocity_std.x); - track_msg.velocity_covariance[1] = detection.absolute_velocity_covariance_xy; + track_msg.velocity_covariance[0] = static_cast(objects.absolute_velocity_std.x); + track_msg.velocity_covariance[1] = objects.absolute_velocity_covariance_xy; track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = static_cast(detection.absolute_velocity_std.y); + track_msg.velocity_covariance[3] = static_cast(objects.absolute_velocity_std.y); track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = static_cast(detection.absolute_velocity_std.z); + track_msg.velocity_covariance[5] = static_cast(objects.absolute_velocity_std.z); - track_msg.acceleration_covariance[0] = - static_cast(detection.absolute_acceleration_std.x); - track_msg.acceleration_covariance[1] = detection.absolute_acceleration_covariance_xy; + track_msg.acceleration_covariance[0] = static_cast(objects.absolute_acceleration_std.x); + track_msg.acceleration_covariance[1] = objects.absolute_acceleration_covariance_xy; track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = - static_cast(detection.absolute_acceleration_std.y); + track_msg.acceleration_covariance[3] = static_cast(objects.absolute_acceleration_std.y); track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = - static_cast(detection.absolute_acceleration_std.z); + track_msg.acceleration_covariance[5] = static_cast(objects.absolute_acceleration_std.z); + + 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); } @@ -514,6 +554,175 @@ radar_msgs::msg::RadarTracks ConvertToRadarTracks( return output_msg; } +visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertToMarkers( + const continental_msgs::msg::ContinentalArs548ObjectList & 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; + + radar_msgs::msg::RadarTrack track_msg; + for (const auto & object : msg.objects) { + const Eigen::Vector2d center_xy{object.position.x, object.position.y}; + const double half_length = 0.5 * object.shape_length_edge_mean; + const double half_width = 0.5 * object.shape_width_edge_mean; + constexpr double DEFAULT_HALF_SIZE = 1.0; + const int reference_index = std::min(object.position_reference, 8); + const double & yaw = object.orientation; + current_ids.emplace(object.object_id); + + visualization_msgs::msg::Marker box_marker; + box_marker.header.frame_id = sensor_cfg_ptr_->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.position.x + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + box_marker.pose.position.y = + object.position.y + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + box_marker.pose.position.z = object.position.z; + 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.age) + "ms"; + + marker_array.markers.emplace_back(text_marker); + + std::stringstream object_status_ss; + object_status_ss << std::fixed << std::setprecision(3) << "ID=" << object.object_id << "\n" + << static_cast(object.status_measurement) << "/" + << static_cast(object.status_movement) << "/" + << static_cast(object.position_reference); + + 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=" << object.object_id + << "\nyaw=" << object.orientation + << "\nyaw_rate=" << object.orientation_rate_mean + << "\nvx=" << object.absolute_velocity.x + << "\nvy=" << object.absolute_velocity.y + << "\nax=" << object.absolute_acceleration.x + << "\nay=" << object.absolute_acceleration.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_ptr_->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; +} + RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548DriverRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 0d7da394b..f4a83ba81 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -53,44 +53,6 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( set_param_res_ = this->add_on_set_parameters_callback( std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); - odometry_sub_ = this->create_subscription( - "odometry_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); - - acceleration_sub_ = create_subscription( - "acceleration_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, std::placeholders::_1)); - - steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1)); - - set_new_sensor_ip_service_server_ = this->create_service( - "set_new_sensor_ip", std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback, - this, std::placeholders::_1, std::placeholders::_2)); - - set_new_sensor_mounting_service_server_ = this->create_service( - "set_new_sensor_mounting", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_new_vehicle_parameters_service_server_ = this->create_service( - "set_new_vehicle_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_new_radar_parameters_service_server_ = this->create_service( - "set_new_radar_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - StreamStart(); } @@ -102,6 +64,48 @@ Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { interface_status_ = hw_interface_.CloudInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + + set_new_sensor_ip_service_server_ = this->create_service( + "set_new_sensor_ip", std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback, + this, std::placeholders::_1, std::placeholders::_2)); + + set_new_sensor_mounting_service_server_ = this->create_service( + "set_new_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_new_vehicle_parameters_service_server_ = this->create_service( + "set_new_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_new_radar_parameters_service_server_ = this->create_service( + "set_new_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); std::scoped_lock lock(mtx_config_); diagnostics_updater_.add( @@ -189,6 +193,15 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("frame_id", descriptor); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -458,32 +471,26 @@ void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback auto tf_buffer = std::make_unique(this->get_clock()); auto tf_listener = std::make_unique(*tf_buffer); - geometry_msgs::msg::TransformStamped autosar_to_sensor_tf; + geometry_msgs::msg::TransformStamped base_to_sensor_tf; try { - autosar_to_sensor_tf = tf_buffer->lookupTransform( - std::string("autosar"), sensor_configuration_.frame_id, rclcpp::Time(0), + base_to_sensor_tf = tf_buffer->lookupTransform( + sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), rclcpp::Duration::from_seconds(0.5)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the autosar frame to %s (%s)", + this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", sensor_configuration_.frame_id.c_str(), ex.what()); return; } - const auto & quat = autosar_to_sensor_tf.transform.rotation; + 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); - std::cout << "DEBUG TF" << std::endl; - std::cout << "translation: x=" << autosar_to_sensor_tf.transform.translation.x - << " y=" << autosar_to_sensor_tf.transform.translation.y - << " z=" << autosar_to_sensor_tf.transform.translation.z << std::endl; - std::cout << "orientation: roll=" << rpy.x << " pitch=" << rpy.y << " yaw=" << rpy.z << std::endl; - hw_interface_.SetSensorMounting( - autosar_to_sensor_tf.transform.translation.x, autosar_to_sensor_tf.transform.translation.y, - autosar_to_sensor_tf.transform.translation.z, rpy.z, rpy.y, - sensor_configuration_.new_plug_orientation); + base_to_sensor_tf.transform.translation.x - sensor_configuration_.new_vehicle_wheelbase, + base_to_sensor_tf.transform.translation.y, base_to_sensor_tf.transform.translation.z, rpy.z, + rpy.y, sensor_configuration_.new_plug_orientation); } void ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback( From cfd2b7ca854a1a8b5260ef05b7603b08a5cb6917 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 6 Feb 2024 11:41:43 +0900 Subject: [PATCH 20/42] fix: had implemented the reference point correction for all relevant interfaces but the radar track msgs Signed-off-by: Kenzo Lobos-Tsunekawa --- ...continental_ars548_decoder_ros_wrapper.cpp | 78 +++++++++++-------- 1 file changed, 45 insertions(+), 33 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 4f9efea8c..cf7aff404 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -484,62 +484,74 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr constexpr float INVALID_COVARIANCE = 1e6; radar_msgs::msg::RadarTrack track_msg; - for (const auto & objects : msg.objects) { - track_msg.uuid.uuid[0] = static_cast(objects.object_id & 0xff); - track_msg.uuid.uuid[1] = static_cast((objects.object_id >> 8) & 0xff); - track_msg.uuid.uuid[2] = static_cast((objects.object_id >> 16) & 0xff); - track_msg.uuid.uuid[3] = static_cast((objects.object_id >> 24) & 0xff); - track_msg.position = objects.position; - track_msg.velocity = objects.absolute_velocity; - track_msg.acceleration = objects.absolute_acceleration; - track_msg.size.x = objects.shape_length_edge_mean; - track_msg.size.y = objects.shape_width_edge_mean; + for (const auto & object : msg.objects) { + 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); + + const double half_length = 0.5 * object.shape_length_edge_mean; + const double half_width = 0.5 * object.shape_width_edge_mean; + const int reference_index = std::min(object.position_reference, 8); + const double & yaw = object.orientation; + track_msg.position.x = object.position.x + + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + track_msg.position.y = object.position.y + + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + track_msg.position.z = object.position.z; + + track_msg.velocity = object.absolute_velocity; + track_msg.acceleration = object.absolute_acceleration; + track_msg.size.x = object.shape_length_edge_mean; + track_msg.size.y = object.shape_width_edge_mean; track_msg.size.z = 1.f; - uint8_t max_score = objects.classification_unknown; + uint8_t max_score = object.classification_unknown; track_msg.classification = UNKNOWN_ID; - if (objects.classification_car > max_score) { - max_score = objects.classification_car; + if (object.classification_car > max_score) { + max_score = object.classification_car; track_msg.classification = CAR_ID; } - if (objects.classification_truck > max_score) { - max_score = objects.classification_truck; + if (object.classification_truck > max_score) { + max_score = object.classification_truck; track_msg.classification = TRUCK_ID; } - if (objects.classification_motorcycle > max_score) { - max_score = objects.classification_motorcycle; + if (object.classification_motorcycle > max_score) { + max_score = object.classification_motorcycle; track_msg.classification = MOTORCYCLE_ID; } - if (objects.classification_bicycle > max_score) { - max_score = objects.classification_bicycle; + if (object.classification_bicycle > max_score) { + max_score = object.classification_bicycle; track_msg.classification = BICYCLE_ID; } - if (objects.classification_pedestrian > max_score) { - max_score = objects.classification_pedestrian; + if (object.classification_pedestrian > max_score) { + max_score = object.classification_pedestrian; track_msg.classification = PEDESTRIAN_ID; } - track_msg.position_covariance[0] = static_cast(objects.position_std.x); - track_msg.position_covariance[1] = objects.position_covariance_xy; + track_msg.position_covariance[0] = static_cast(object.position_std.x); + track_msg.position_covariance[1] = object.position_covariance_xy; track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = static_cast(objects.position_std.y); + track_msg.position_covariance[3] = static_cast(object.position_std.y); track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = static_cast(objects.position_std.z); + track_msg.position_covariance[5] = static_cast(object.position_std.z); - track_msg.velocity_covariance[0] = static_cast(objects.absolute_velocity_std.x); - track_msg.velocity_covariance[1] = objects.absolute_velocity_covariance_xy; + track_msg.velocity_covariance[0] = static_cast(object.absolute_velocity_std.x); + track_msg.velocity_covariance[1] = object.absolute_velocity_covariance_xy; track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = static_cast(objects.absolute_velocity_std.y); + track_msg.velocity_covariance[3] = static_cast(object.absolute_velocity_std.y); track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = static_cast(objects.absolute_velocity_std.z); + track_msg.velocity_covariance[5] = static_cast(object.absolute_velocity_std.z); - track_msg.acceleration_covariance[0] = static_cast(objects.absolute_acceleration_std.x); - track_msg.acceleration_covariance[1] = objects.absolute_acceleration_covariance_xy; + track_msg.acceleration_covariance[0] = static_cast(object.absolute_acceleration_std.x); + track_msg.acceleration_covariance[1] = object.absolute_acceleration_covariance_xy; track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = static_cast(objects.absolute_acceleration_std.y); + track_msg.acceleration_covariance[3] = static_cast(object.absolute_acceleration_std.y); track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = static_cast(objects.absolute_acceleration_std.z); + track_msg.acceleration_covariance[5] = static_cast(object.absolute_acceleration_std.z); track_msg.size_covariance[0] = INVALID_COVARIANCE; track_msg.size_covariance[1] = 0.f; From 098f400e3471fdc437090eac4186a95f67268603 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 16 Feb 2024 18:08:44 +0900 Subject: [PATCH 21/42] feat: implemented changes from another branch (for the ars548). dianostics are now parsed in the decoder, fixed radar configuration, and implemented a temporary multi radar hw interface Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 306 ++++++++++-- .../continental/continental_common.hpp | 84 ---- .../decoders/continental_ars548_decoder.hpp | 34 +- .../decoders/continental_packets_decoder.hpp | 5 - nebula_decoders/package.xml | 1 + .../decoders/continental_ars548_decoder.cpp | 246 +++++++++- nebula_hw_interfaces/CMakeLists.txt | 1 + .../nebula_hw_interface_base.hpp | 6 +- .../continental_ars548_hw_interface.hpp | 42 +- .../continental_types.hpp | 152 ------ .../multi_continental_ars548_hw_interface.hpp | 179 +++++++ .../hesai_hw_interface.hpp | 79 +-- .../robosense_hw_interface.hpp | 26 +- .../velodyne_hw_interface.hpp | 22 +- .../continental_ars548_hw_interface.cpp | 232 +-------- .../multi_continental_ars548_hw_interface.cpp | 453 ++++++++++++++++++ .../hesai_hw_interface.cpp | 251 +++++----- .../robosense_hw_interface.cpp | 27 +- .../velodyne_hw_interface.cpp | 22 +- nebula_ros/CMakeLists.txt | 9 +- .../nebula_hw_interface_ros_wrapper_base.hpp | 16 +- ...continental_ars548_decoder_ros_wrapper.hpp | 15 +- ...nental_ars548_hw_interface_ros_wrapper.hpp | 14 +- ...nental_ars548_hw_interface_ros_wrapper.hpp | 143 ++++++ .../hesai/hesai_decoder_ros_wrapper.hpp | 20 +- .../hesai/hesai_hw_interface_ros_wrapper.hpp | 21 +- .../hesai/hesai_hw_monitor_ros_wrapper.hpp | 23 +- .../robosense_hw_interface_ros_wrapper.hpp | 2 +- .../velodyne_hw_interface_ros_wrapper.hpp | 19 +- .../launch/continental_launch_all_hw.xml | 223 ++++++--- ...continental_ars548_decoder_ros_wrapper.cpp | 151 +++++- ...nental_ars548_hw_interface_ros_wrapper.cpp | 123 ++--- ...nental_ars548_hw_interface_ros_wrapper.cpp | 345 +++++++++++++ .../src/hesai/hesai_decoder_ros_wrapper.cpp | 237 +++++---- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 74 +-- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 45 +- .../robosense_hw_interface_ros_wrapper.cpp | 2 +- .../velodyne_hw_interface_ros_wrapper.cpp | 27 +- 38 files changed, 2668 insertions(+), 1009 deletions(-) delete mode 100644 nebula_common/include/nebula_common/continental/continental_common.hpp delete mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp create mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp create mode 100644 nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index d2d3246b6..e4e3f5178 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -13,17 +13,27 @@ // limitations under the License. #pragma once -/** - * Continental ARS548 - */ + +#include +#include + #include "boost/endian/buffers.hpp" +#include +#include #include #include +#include +#include #include #include #include +#include +#include +#include +#include +#include namespace nebula { @@ -32,6 +42,230 @@ namespace drivers namespace continental_ars548 { +/// @brief struct for ARS548 sensor configuration +struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase +{ + std::string multicast_ip{}; + std::string new_sensor_ip{}; + std::string base_frame{}; + uint16_t configuration_host_port{}; + uint16_t configuration_sensor_port{}; + bool use_sensor_time{}; + uint16_t new_plug_orientation{}; + float new_vehicle_length{}; + float new_vehicle_width{}; + float new_vehicle_height{}; + float new_vehicle_wheelbase{}; + uint16_t new_radar_maximum_distance{}; + uint16_t new_radar_frequency_slot{}; + uint16_t new_radar_cycle_time{}; + uint16_t new_radar_time_slot{}; + uint16_t new_radar_country_code{}; + uint16_t new_radar_powersave_standstill{}; +}; + +/// @brief struct for Multiple ARS548 sensor configuration +struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration +{ + std::vector sensor_ips{}; + std::vector frame_ids{}; +}; + +/// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, ContinentalARS548SensorConfiguration const & arg) +{ + os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip + << ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame + << ", ConfigurationHostPort: " << arg.configuration_host_port + << ", ConfigurationSensorPort: " << arg.configuration_sensor_port + << ", UseSensorTime: " << arg.use_sensor_time + << ", NewPlugOrientation: " << arg.new_plug_orientation + << ", NewVehicleLength: " << arg.new_vehicle_length + << ", NewVehicleWidth: " << arg.new_vehicle_width + << ", NewVehicleHeight: " << arg.new_vehicle_height + << ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase + << ", NewRadarMaximumDistance: " << arg.new_radar_maximum_distance + << ", NewRadarFrequencySlot: " << arg.new_radar_frequency_slot + << ", NewRadarCycleTime: " << arg.new_radar_cycle_time + << ", NewRadarTimeSlot: " << arg.new_radar_time_slot + << ", NewRadarCountryCode: " << arg.new_radar_country_code + << ", RadarPowersaveStandstill: " << arg.new_radar_powersave_standstill; + return os; +} + +/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +inline std::ostream & operator<<( + std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg) +{ + std::stringstream sensor_ips_ss; + sensor_ips_ss << "["; + for (const auto sensor_ip : arg.sensor_ips) { + sensor_ips_ss << sensor_ip << ", "; + } + sensor_ips_ss << "]"; + + std::stringstream frame_ids_ss; + frame_ids_ss << "["; + for (const auto frame_id : arg.frame_ids) { + frame_ids_ss << frame_id << ", "; + } + frame_ids_ss << "]"; + + os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip + << ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str(); + return os; +} + +/// @brief semantic struct of ARS548 Status +struct ContinentalARS548Status +{ + // Filled with raw sensor data + uint32_t timestamp_nanoseconds{}; + uint32_t timestamp_seconds{}; + std::string timestamp_sync_status{}; + uint8_t sw_version_major{}; + uint8_t sw_version_minor{}; + uint8_t sw_version_patch{}; + float longitudinal{}; + float lateral{}; + float vertical{}; + float yaw{}; + float pitch{}; + std::string plug_orientation{}; + float length{}; + float width{}; + float height{}; + float wheel_base{}; + uint16_t max_distance{}; + std::string frequency_slot{}; + uint8_t cycle_time{}; + uint8_t time_slot{}; + std::string hcc{}; + std::string power_save_standstill{}; + std::string sensor_ip_address0{}; + std::string sensor_ip_address1{}; + uint8_t configuration_counter{}; + std::string longitudinal_velocity_status{}; + std::string longitudinal_acceleration_status{}; + std::string lateral_acceleration_status{}; + std::string yaw_rate_status{}; + std::string steering_angle_status{}; + std::string driving_direction_status{}; + std::string characteristic_speed_status{}; + std::string radar_status{}; + std::string voltage_status{}; + std::string temperature_status{}; + std::string blockage_status{}; + + // Processed data + uint64_t detection_first_stamp{}; + uint64_t detection_last_stamp{}; + uint64_t detection_total_count{}; + uint64_t detection_dropped_dt_count{}; + uint64_t detection_empty_count{}; + + uint64_t object_first_stamp{}; + uint64_t object_last_stamp{}; + uint64_t object_total_count{}; + uint64_t object_dropped_dt_count{}; + uint64_t object_empty_count{}; + + uint64_t status_total_count{}; + uint64_t radar_invalid_count{}; + + ContinentalARS548Status() {} + + /// @brief Stream ContinentalRadarStatus method + /// @param os + /// @param arg + /// @return stream + friend std::ostream & operator<<(std::ostream & os, ContinentalARS548Status const & arg) + { + os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; + os << ", "; + os << "timestamp_seconds: " << arg.timestamp_seconds; + os << ", "; + os << "timestamp_sync_status: " << arg.timestamp_sync_status; + os << ", "; + os << "sw_version_major: " << arg.sw_version_major; + os << ", "; + os << "sw_version_minor: " << arg.sw_version_minor; + os << ", "; + os << "sw_version_patch: " << arg.sw_version_patch; + os << ", "; + os << "longitudinal: " << arg.longitudinal; + os << ", "; + os << "lateral: " << arg.lateral; + os << ", "; + os << "vertical: " << arg.vertical; + os << ", "; + os << "yaw: " << arg.yaw; + os << ", "; + os << "pitch: " << arg.pitch; + os << ", "; + os << "plug_orientation: " << arg.plug_orientation; + os << ", "; + os << "length: " << arg.length; + os << ", "; + os << "width: " << arg.width; + os << ", "; + os << "height: " << arg.height; + os << ", "; + os << "wheel_base: " << arg.wheel_base; + os << ", "; + os << "max_distance: " << arg.max_distance; + os << ", "; + os << "frequency_slot: " << arg.frequency_slot; + os << ", "; + os << "cycle_time: " << arg.cycle_time; + os << ", "; + os << "time_slot: " << arg.time_slot; + os << ", "; + os << "hcc: " << arg.hcc; + os << ", "; + os << "power_save_standstill: " << arg.power_save_standstill; + os << ", "; + os << "sensor_ip_address0: " << arg.sensor_ip_address0; + os << ", "; + os << "sensor_ip_address1: " << arg.sensor_ip_address1; + os << ", "; + os << "configuration_counter: " << arg.configuration_counter; + os << ", "; + os << "status_longitudinal_velocity: " << arg.longitudinal_velocity_status; + os << ", "; + os << "status_longitudinal_acceleration: " << arg.longitudinal_acceleration_status; + os << ", "; + os << "status_lateral_acceleration: " << arg.lateral_acceleration_status; + os << ", "; + os << "status_yaw_rate: " << arg.yaw_rate_status; + os << ", "; + os << "status_steering_angle: " << arg.steering_angle_status; + os << ", "; + os << "status_driving_direction: " << arg.driving_direction_status; + os << ", "; + os << "characteristic_speed: " << arg.characteristic_speed_status; + os << ", "; + os << "radar_status: " << arg.radar_status; + os << ", "; + os << "temperature_status: " << arg.temperature_status; + os << ", "; + os << "voltage_status: " << arg.voltage_status; + os << ", "; + os << "blockage_status: " << arg.blockage_status; + + return os; + } +}; + using boost::endian::big_float32_buf_t; using boost::endian::big_uint16_buf_t; using boost::endian::big_uint32_buf_t; @@ -66,9 +300,9 @@ constexpr int MAX_OBJECTS = 50; struct HeaderPacket { - big_uint16_buf_t service_id; - big_uint16_buf_t method_id; - big_uint32_buf_t length; + big_uint16_buf_t service_id{}; + big_uint16_buf_t method_id{}; + big_uint32_buf_t length{}; }; struct HeaderSomeIPPacket @@ -231,30 +465,30 @@ struct ObjectListPacket struct StatusConfigurationPacket { - big_float32_buf_t longitudinal; - big_float32_buf_t lateral; - big_float32_buf_t vertical; - big_float32_buf_t yaw; - big_float32_buf_t pitch; - uint8_t plug_orientation; - big_float32_buf_t length; - big_float32_buf_t width; - big_float32_buf_t height; - big_float32_buf_t wheelbase; - big_uint16_buf_t maximum_distance; - uint8_t frequency_slot; - uint8_t cycle_time; - uint8_t time_slot; - uint8_t hcc; - uint8_t powersave_standstill; - uint8_t sensor_ip_address00; - uint8_t sensor_ip_address01; - uint8_t sensor_ip_address02; - uint8_t sensor_ip_address03; - uint8_t sensor_ip_address10; - uint8_t sensor_ip_address11; - uint8_t sensor_ip_address12; - uint8_t sensor_ip_address13; + big_float32_buf_t longitudinal{}; + big_float32_buf_t lateral{}; + big_float32_buf_t vertical{}; + big_float32_buf_t yaw{}; + big_float32_buf_t pitch{}; + uint8_t plug_orientation{}; + big_float32_buf_t length{}; + big_float32_buf_t width{}; + big_float32_buf_t height{}; + big_float32_buf_t wheelbase{}; + big_uint16_buf_t maximum_distance{}; + uint8_t frequency_slot{}; + uint8_t cycle_time{}; + uint8_t time_slot{}; + uint8_t hcc{}; + uint8_t powersave_standstill{}; + uint8_t sensor_ip_address00{}; + uint8_t sensor_ip_address01{}; + uint8_t sensor_ip_address02{}; + uint8_t sensor_ip_address03{}; + uint8_t sensor_ip_address10{}; + uint8_t sensor_ip_address11{}; + uint8_t sensor_ip_address12{}; + uint8_t sensor_ip_address13{}; }; struct SensorStatusPacket @@ -281,12 +515,12 @@ struct SensorStatusPacket struct ConfigurationPacket { - HeaderPacket header; - StatusConfigurationPacket configuration; - uint8_t new_sensor_mounting; - uint8_t new_vehicle_parameters; - uint8_t new_radar_parameters; - uint8_t new_network_configuration; + HeaderPacket header{}; + StatusConfigurationPacket configuration{}; + uint8_t new_sensor_mounting{}; + uint8_t new_vehicle_parameters{}; + uint8_t new_radar_parameters{}; + uint8_t new_network_configuration{}; }; struct AccelerationLateralCoGPacket diff --git a/nebula_common/include/nebula_common/continental/continental_common.hpp b/nebula_common/include/nebula_common/continental/continental_common.hpp deleted file mode 100644 index ea4d5e949..000000000 --- a/nebula_common/include/nebula_common/continental/continental_common.hpp +++ /dev/null @@ -1,84 +0,0 @@ - -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NEBULA_CONTINENTAL_COMMON_H -#define NEBULA_CONTINENTAL_COMMON_H - -#include "nebula_common/nebula_common.hpp" -#include "nebula_common/nebula_status.hpp" - -#include -#include -#include -#include -#include -#include -namespace nebula -{ -namespace drivers -{ -/// @brief struct for ARS548 sensor configuration -struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase -{ - std::string multicast_ip{}; - std::string new_sensor_ip{}; - std::string base_frame{}; - uint16_t configuration_host_port{}; - uint16_t configuration_sensor_port{}; - bool use_sensor_time{}; - uint16_t new_plug_orientation{}; - float new_vehicle_length{}; - float new_vehicle_width{}; - float new_vehicle_height{}; - float new_vehicle_wheelbase{}; - uint16_t new_radar_maximum_distance{}; - uint16_t new_radar_frequency_slot{}; - uint16_t new_radar_cycle_time{}; - uint16_t new_radar_time_slot{}; - uint16_t new_radar_country_code{}; - uint16_t new_radar_powersave_standstill{}; -}; - -/// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << -/// operator) -/// @param os -/// @param arg -/// @return stream -inline std::ostream & operator<<( - std::ostream & os, ContinentalARS548SensorConfiguration const & arg) -{ - os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame - << ", ConfigurationHostPort: " << arg.configuration_host_port - << ", ConfigurationSensorPort: " << arg.configuration_sensor_port - << ", UseSensorTime: " << arg.use_sensor_time - << ", NewPlugOrientation: " << arg.new_plug_orientation - << ", NewVehicleLength: " << arg.new_vehicle_length - << ", NewVehicleWidth: " << arg.new_vehicle_width - << ", NewVehicleHeight: " << arg.new_vehicle_height - << ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase - << ", NewRadarMaximumDistance: " << arg.new_radar_maximum_distance - << ", NewRadarFrequencySlot: " << arg.new_radar_frequency_slot - << ", NewRadarCycleTime: " << arg.new_radar_cycle_time - << ", NewRadarTimeSlot: " << arg.new_radar_time_slot - << ", NewRadarCountryCode: " << arg.new_radar_country_code - << ", RadarPowersaveStandstill: " << arg.new_radar_powersave_standstill; - return os; -} - -} // namespace drivers -} // namespace nebula - -#endif // NEBULA_CONTINENTAL_COMMON_H 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 fe8abcecc..139ea6d5e 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,11 +14,12 @@ #pragma once -#include +#include #include #include #include +#include #include #include #include @@ -40,37 +41,62 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @brief Constructor /// @param sensor_configuration SensorConfiguration for this decoder explicit ContinentalARS548Decoder( - const std::shared_ptr & sensor_configuration); + const std::shared_ptr & sensor_configuration); /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag bool ProcessPackets(const nebula_msgs::msg::NebulaPackets & nebula_packets) override; + /// @brief Function for parsing detection lists + /// @param data + /// @return Resulting flag bool ParseDetectionsListPacket( const std::vector & data, const std_msgs::msg::Header & header); + + /// @brief Function for parsing object lists + /// @param data + /// @return Resulting flag bool ParseObjectsListPacket( const std::vector & data, const std_msgs::msg::Header & header); - /// @brief Register function to call whenever a new detection list is processed + /// @brief Function for parsing sensor status messages + /// @param data + /// @return Resulting flag + bool ParseSensorStatusPacket( + const std::vector & data, const std_msgs::msg::Header & header); + + /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback /// @return Resulting status Status RegisterDetectionListCallback( std::function)> detection_list_callback); - /// @brief Register function to call whenever a new object list is processed + /// @brief Register function to call when 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 when a new sensor status message is processed + /// @param object_list_callback + /// @return Resulting status + Status RegisterSensorStatusCallback( + std::function sensor_status_callback); + private: std::function msg)> detection_list_callback_; std::function msg)> object_list_callback_; + std::function sensor_status_callback_; + + ContinentalARS548Status radar_status_{}; + + /// @brief SensorConfiguration for this decoder + std::shared_ptr sensor_configuration_; }; } // namespace continental_ars548 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index 9efca35b8..e5f0f5efa 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,7 +15,6 @@ #ifndef NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP #define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_HPP -#include "nebula_common/continental/continental_common.hpp" #include "nebula_common/point_types.hpp" #include "nebula_msgs/msg/nebula_packet.hpp" @@ -31,10 +30,6 @@ namespace drivers /// @brief Base class for Continental Radar decoder class ContinentalPacketsDecoder { -protected: - /// @brief SensorConfiguration for this decoder - std::shared_ptr sensor_configuration_; - public: ContinentalPacketsDecoder(ContinentalPacketsDecoder && c) = delete; ContinentalPacketsDecoder & operator=(ContinentalPacketsDecoder && c) = delete; diff --git a/nebula_decoders/package.xml b/nebula_decoders/package.xml index 0751cbe7f..462f3956e 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -14,6 +14,7 @@ angles continental_msgs + diagnostic_msgs libpcl-all-dev nebula_common nebula_msgs 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 a63436979..1b3807822 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 @@ -15,7 +15,6 @@ #include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" #include "nebula_common/continental/continental_ars548.hpp" -#include "nebula_common/continental/continental_common.hpp" #include #include @@ -27,7 +26,8 @@ namespace drivers namespace continental_ars548 { ContinentalARS548Decoder::ContinentalARS548Decoder( - const std::shared_ptr & sensor_configuration) + const std::shared_ptr & + sensor_configuration) { sensor_configuration_ = sensor_configuration; } @@ -48,6 +48,13 @@ Status ContinentalARS548Decoder::RegisterObjectListCallback( return Status::OK; } +Status ContinentalARS548Decoder::RegisterSensorStatusCallback( + std::function sensor_status_callback) +{ + sensor_status_callback_ = std::move(sensor_status_callback); + return Status::OK; +} + bool ContinentalARS548Decoder::ProcessPackets( const nebula_msgs::msg::NebulaPackets & nebula_packets) { @@ -82,6 +89,14 @@ bool ContinentalARS548Decoder::ProcessPackets( } return ParseObjectsListPacket(data, nebula_packets.header); + } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { + if ( + data.size() != SENSOR_STATUS_UDP_PAYLOAD || + header.length.value() != SENSOR_STATUS_PDU_LENGTH) { + return false; + } + + return ParseSensorStatusPacket(data, nebula_packets.header); } return true; @@ -130,6 +145,24 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( const uint32_t number_of_detections = detection_list.number_of_detections.value(); msg.detections.resize(number_of_detections); + if (radar_status_.timestamp_sync_status == "SYNC_OK") { + if (radar_status_.detection_first_stamp == 0) { + radar_status_.detection_first_stamp = + static_cast(msg.header.stamp.sec) * 1'000'000'000 + + static_cast(msg.header.stamp.nanosec); + radar_status_.detection_last_stamp = radar_status_.detection_first_stamp; + } else { + uint64_t stamp = static_cast(msg.header.stamp.sec) * 1'000'000'000 + + static_cast(msg.header.stamp.nanosec); + radar_status_.detection_total_count++; + radar_status_.detection_empty_count += number_of_detections == 0 ? 1 : 0; + radar_status_.detection_dropped_dt_count += + (stamp - radar_status_.detection_last_stamp > 1.75 * radar_status_.cycle_time * 10e6) ? 1 + : 0; + radar_status_.detection_last_stamp = stamp; + } + } + assert(msg.origin_pos.x >= -10.f && msg.origin_pos.x <= 10.f); assert(msg.origin_pos.y >= -10.f && msg.origin_pos.y <= 10.f); assert(msg.origin_pos.z >= -10.f && msg.origin_pos.z <= 10.f); @@ -220,6 +253,23 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( msg.objects.resize(number_of_objects); + if (radar_status_.timestamp_sync_status == "SYNC_OK") { + if (radar_status_.object_first_stamp == 0) { + radar_status_.object_first_stamp = + static_cast(msg.header.stamp.sec) * 1'000'000'000 + + static_cast(msg.header.stamp.nanosec); + radar_status_.object_last_stamp = radar_status_.object_first_stamp; + } else { + uint64_t stamp = static_cast(msg.header.stamp.sec) * 1'000'000'000 + + static_cast(msg.header.stamp.nanosec); + radar_status_.object_total_count++; + radar_status_.object_empty_count += number_of_objects == 0 ? 1 : 0; + radar_status_.object_dropped_dt_count += + (stamp - radar_status_.object_last_stamp > 1.75 * radar_status_.cycle_time * 10e6) ? 1 : 0; + radar_status_.object_last_stamp = stamp; + } + } + for (std::size_t object_index = 0; object_index < number_of_objects; object_index++) { auto & object_msg = msg.objects[object_index]; const ObjectPacket & object = object_list.objects[object_index]; @@ -327,6 +377,198 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( return true; } +bool ContinentalARS548Decoder::ParseSensorStatusPacket( + const std::vector & data, const std_msgs::msg::Header & header) +{ + SensorStatusPacket sensor_status_packet; + std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket)); + + radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); + radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); + + if (sensor_status_packet.stamp.timestamp_sync_status == 1) { + radar_status_.timestamp_sync_status = "SYNC_OK"; + } else if (sensor_status_packet.stamp.timestamp_sync_status == 2) { + radar_status_.timestamp_sync_status = "NEVER_SYNC"; + } else if (sensor_status_packet.stamp.timestamp_sync_status == 3) { + radar_status_.timestamp_sync_status = "SYNC_LOST"; + } else { + radar_status_.timestamp_sync_status = "INVALID_VALUE"; + } + + radar_status_.sw_version_major = sensor_status_packet.sw_version_major; + radar_status_.sw_version_minor = sensor_status_packet.sw_version_minor; + radar_status_.sw_version_patch = sensor_status_packet.sw_version_patch; + + radar_status_.plug_orientation = sensor_status_packet.status.plug_orientation == 0 ? "PLUG_RIGHT" + : sensor_status_packet.status.plug_orientation == 1 + ? "PLUG_LEFT" + : "INVALID_VALUE"; + + radar_status_.max_distance = sensor_status_packet.status.maximum_distance.value(); + + radar_status_.longitudinal = sensor_status_packet.status.longitudinal.value(); + radar_status_.lateral = sensor_status_packet.status.lateral.value(); + radar_status_.vertical = sensor_status_packet.status.vertical.value(); + radar_status_.yaw = sensor_status_packet.status.yaw.value(); + + radar_status_.pitch = sensor_status_packet.status.pitch.value(); + ; + radar_status_.length = sensor_status_packet.status.length.value(); + ; + radar_status_.width = sensor_status_packet.status.width.value(); + radar_status_.height = sensor_status_packet.status.height.value(); + radar_status_.wheel_base = sensor_status_packet.status.wheelbase.value(); + + if (sensor_status_packet.status.frequency_slot == 0) { + radar_status_.frequency_slot = "0:Low (76.23 GHz)"; + } else if (sensor_status_packet.status.frequency_slot == 1) { + radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; + } else if (sensor_status_packet.status.frequency_slot == 2) { + radar_status_.frequency_slot = "2:High (76.73 GHz)"; + } else { + radar_status_.frequency_slot = "INVALID VALUE"; + } + + radar_status_.cycle_time = sensor_status_packet.status.cycle_time; + radar_status_.time_slot = sensor_status_packet.status.time_slot; + + radar_status_.hcc = sensor_status_packet.status.hcc == 1 ? "Worldwide" + : sensor_status_packet.status.hcc == 2 + ? "Japan" + : ("INVALID VALUE=" + std::to_string(sensor_status_packet.status.hcc)); + + radar_status_.power_save_standstill = + sensor_status_packet.status.powersave_standstill == 0 ? "Off" + : sensor_status_packet.status.powersave_standstill == 1 ? "On" + : "INVALID VALUE"; + + std::stringstream ss0, ss1; + ss0 << std::to_string(sensor_status_packet.status.sensor_ip_address00) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address01) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address02) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address03); + radar_status_.sensor_ip_address0 = ss0.str(); + + ss1 << std::to_string(sensor_status_packet.status.sensor_ip_address10) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address11) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address12) << "." + << std::to_string(sensor_status_packet.status.sensor_ip_address13); + radar_status_.sensor_ip_address1 = ss1.str(); + + radar_status_.configuration_counter = sensor_status_packet.configuration_counter; + + radar_status_.longitudinal_velocity_status = + sensor_status_packet.longitudinal_velocity_status == 0 ? "VDY_OK" + : sensor_status_packet.longitudinal_velocity_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.longitudinal_acceleration_status = + sensor_status_packet.longitudinal_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet.longitudinal_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.lateral_acceleration_status = + sensor_status_packet.lateral_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet.lateral_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.yaw_rate_status = sensor_status_packet.yaw_rate_status == 0 ? "VDY_OK" + : sensor_status_packet.yaw_rate_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.steering_angle_status = sensor_status_packet.steering_angle_status == 0 ? "VDY_OK" + : sensor_status_packet.steering_angle_status == 1 + ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.driving_direction_status = + sensor_status_packet.driving_direction_status == 0 ? "VDY_OK" + : sensor_status_packet.driving_direction_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.characteristic_speed_status = + sensor_status_packet.characteristic_speed_status == 0 ? "VDY_OK" + : sensor_status_packet.characteristic_speed_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + if (sensor_status_packet.radar_status == 0) { + radar_status_.radar_status = "STATE_INIT"; + } else if (sensor_status_packet.radar_status == 1) { + radar_status_.radar_status = "STATE_OK"; + } else if (sensor_status_packet.radar_status == 2) { + radar_status_.radar_status = "STATE_INVALID"; + } else { + radar_status_.radar_status = "INVALID VALUE"; + } + + if (sensor_status_packet.voltage_status == 0) { + radar_status_.voltage_status = "Ok"; + } + if (sensor_status_packet.voltage_status & 0x01) { + radar_status_.voltage_status += "Current under voltage"; + } + if (sensor_status_packet.voltage_status & 0x02) { + radar_status_.voltage_status = "Past under voltage"; + } + if (sensor_status_packet.voltage_status & 0x03) { + radar_status_.voltage_status = "Current over voltage"; + } + if (sensor_status_packet.voltage_status & 0x04) { + radar_status_.voltage_status = "Past over voltage"; + } + + if (sensor_status_packet.temperature_status == 0) { + radar_status_.temperature_status = "Ok"; + } + if (sensor_status_packet.temperature_status & 0x01) { + radar_status_.temperature_status += "Current under temperature"; + } + if (sensor_status_packet.temperature_status & 0x02) { + radar_status_.temperature_status += "Past under temperature"; + } + if (sensor_status_packet.temperature_status & 0x03) { + radar_status_.temperature_status += "Current over temperature"; + } + if (sensor_status_packet.temperature_status & 0x04) { + radar_status_.temperature_status += "Past over temperature"; + } + + const uint8_t & blockage_status0 = sensor_status_packet.blockage_status & 0x0f; + const uint8_t & blockage_status1 = (sensor_status_packet.blockage_status & 0xf0) >> 4; + + if (blockage_status0 == 0) { + radar_status_.blockage_status = "Blind"; + } else if (blockage_status0 == 1) { + radar_status_.blockage_status = "High"; + } else if (blockage_status0 == 2) { + radar_status_.blockage_status = "Mid"; + } else if (blockage_status0 == 3) { + radar_status_.blockage_status = "Low"; + } else if (blockage_status0 == 4) { + radar_status_.blockage_status = "None"; + } else { + radar_status_.blockage_status = "INVALID VALUE"; + } + + if (blockage_status1 == 0) { + radar_status_.blockage_status += ". Self test failed"; + } else if (blockage_status1 == 1) { + radar_status_.blockage_status += ". Self test passed"; + } else if (blockage_status1 == 2) { + radar_status_.blockage_status += ". Self test ongoing"; + } else { + radar_status_.blockage_status += ". Invalid self test"; + } + + radar_status_.status_total_count++; + radar_status_.radar_invalid_count += sensor_status_packet.radar_status == 2 ? 1 : 0; + + sensor_status_callback_(radar_status_); + + return true; +} + } // namespace continental_ars548 } // namespace drivers } // namespace nebula diff --git a/nebula_hw_interfaces/CMakeLists.txt b/nebula_hw_interfaces/CMakeLists.txt index 7cd8b8982..32893d1f4 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -40,6 +40,7 @@ ament_auto_add_library(nebula_hw_interfaces_robosense SHARED ament_auto_add_library(nebula_hw_interfaces_continental SHARED src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp + src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp ) diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index c3b405a74..9c73e0ac4 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -37,7 +37,7 @@ class NebulaHwInterfaceBase * @param buffer Buffer containing the data received from the UDP socket * @return Status::OK if no error occurred. */ - virtual void ReceiveCloudPacketCallback(const std::vector & buffer) = 0; + virtual void ReceiveSensorPacketCallback([[maybe_unused]] const std::vector & buffer) {} // virtual Status RegisterScanCallback( // std::function>>)> scan_callback) = 0; @@ -51,11 +51,11 @@ class NebulaHwInterfaceBase /// @brief Virtual function for starting the interface that handles UDP streams /// @return Resulting status - virtual Status CloudInterfaceStart() = 0; + virtual Status SensorInterfaceStart() = 0; /// @brief Virtual function for stopping the interface that handles UDP streams /// @return Resulting status - virtual Status CloudInterfaceStop() = 0; + virtual Status SensorInterfaceStop() = 0; // You may want to also implement GpsInterfaceStart() and ReceiveGpsCallback, but that is sensor // specific. 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 cccbd0c69..3e150b18e 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NEBULA_CONTINENTAL_RADAR_ETHERNET_HW_INTERFACE_H -#define NEBULA_CONTINENTAL_RADAR_ETHERNET_HW_INTERFACE_H +#ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H // Have to define macros to silence warnings about deprecated headers being used by // boost/property_tree/ in some versions of boost. // See: https://github.com/boostorg/property_tree/issues/51 @@ -24,18 +24,15 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" -#include "nebula_common/continental/continental_ars548.hpp" -#include "nebula_common/continental/continental_common.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" -#include "nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp" - +#include +#include +#include +#include +#include #include -#include "nebula_msgs/msg/nebula_packet.hpp" -#include "nebula_msgs/msg/nebula_packets.hpp" +#include +#include #include #include @@ -56,7 +53,7 @@ namespace continental_ars548 class ContinentalARS548HwInterface : NebulaHwInterfaceBase { private: - std::unique_ptr<::drivers::common::IoContext> cloud_io_context_; + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; std::shared_ptr sensor_configuration_; std::unique_ptr nebula_packets_ptr_; @@ -67,7 +64,6 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase SensorStatusPacket sensor_status_packet_{}; FilterStatusPacket filter_status_{}; - ContinentalARS548Status radar_status_{}; std::shared_ptr parent_node_logger; @@ -91,10 +87,6 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @brief Constructor ContinentalARS548HwInterface(); - /// @brief Process a new sensor status packet - /// @param buffer The buffer containing the status packet - void ProcessSensorStatusPacket(const std::vector & buffer); - /// @brief Process a new filter status packet /// @param buffer The buffer containing the status packet void ProcessFilterStatusPacket(const std::vector & buffer); @@ -105,20 +97,20 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveCloudPacketCallbackWithSender( + void ReceiveSensorPacketCallbackWithSender( const std::vector & buffer, const std::string & sender_ip); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveCloudPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStart() final; + Status SensorInterfaceStart() final; /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStop() final; + Status SensorInterfaceStop() final; /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface @@ -215,10 +207,6 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase /// @return Resulting status Status CheckAndSetConfig(); - /// @brief Returns the last semantic sensor status - /// @return Last semantic sensor status message - ContinentalARS548Status GetRadarStatus(); - /// @brief Setting rclcpp::Logger /// @param node Logger void SetLogger(std::shared_ptr node); @@ -227,4 +215,4 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase } // namespace drivers } // namespace nebula -#endif // NEBULA_CONTINENTAL_RADAR_ETHERNET_HW_INTERFACE_H +#endif // NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp deleted file mode 100644 index 7a7485f75..000000000 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright 2024 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CONTINENTAL_TYPES_HPP -#define CONTINENTAL_TYPES_HPP - -#include -#include - -#include -#include - -namespace nebula -{ - -/// @brief semantic struct of ARS548 Status -struct ContinentalARS548Status -{ - uint32_t timestamp_nanoseconds; - uint32_t timestamp_seconds; - std::string timestamp_sync_status; - uint8_t sw_version_major; - uint8_t sw_version_minor; - uint8_t sw_version_patch; - float longitudinal; - float lateral; - float vertical; - float yaw; - float pitch; - std::string plug_orientation; - float length; - float width; - float height; - float wheel_base; - uint16_t max_distance; - std::string frequency_slot; - uint8_t cycle_time; - uint8_t time_slot; - std::string hcc; - std::string power_save_standstill; - std::string sensor_ip_address0; - std::string sensor_ip_address1; - uint8_t configuration_counter; - std::string longitudinal_velocity_status; - std::string longitudinal_acceleration_status; - std::string lateral_acceleration_status; - std::string yaw_rate_status; - std::string steering_angle_status; - std::string driving_direction_status; - std::string characteristic_speed_status; - std::string radar_status; - std::string voltage_status; - std::string temperature_status; - std::string blockage_status; - - ContinentalARS548Status() {} - - /// @brief Stream ContinentalRadarStatus method - /// @param os - /// @param arg - /// @return stream - friend std::ostream & operator<<(std::ostream & os, nebula::ContinentalARS548Status const & arg) - { - os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds; - os << ", "; - os << "timestamp_seconds: " << arg.timestamp_seconds; - os << ", "; - os << "timestamp_sync_status: " << arg.timestamp_sync_status; - os << ", "; - os << "sw_version_major: " << arg.sw_version_major; - os << ", "; - os << "sw_version_minor: " << arg.sw_version_minor; - os << ", "; - os << "sw_version_patch: " << arg.sw_version_patch; - os << ", "; - os << "longitudinal: " << arg.longitudinal; - os << ", "; - os << "lateral: " << arg.lateral; - os << ", "; - os << "vertical: " << arg.vertical; - os << ", "; - os << "yaw: " << arg.yaw; - os << ", "; - os << "pitch: " << arg.pitch; - os << ", "; - os << "plug_orientation: " << arg.plug_orientation; - os << ", "; - os << "length: " << arg.length; - os << ", "; - os << "width: " << arg.width; - os << ", "; - os << "height: " << arg.height; - os << ", "; - os << "wheel_base: " << arg.wheel_base; - os << ", "; - os << "max_distance: " << arg.max_distance; - os << ", "; - os << "frequency_slot: " << arg.frequency_slot; - os << ", "; - os << "cycle_time: " << arg.cycle_time; - os << ", "; - os << "time_slot: " << arg.time_slot; - os << ", "; - os << "hcc: " << arg.hcc; - os << ", "; - os << "power_save_standstill: " << arg.power_save_standstill; - os << ", "; - os << "sensor_ip_address0: " << arg.sensor_ip_address0; - os << ", "; - os << "sensor_ip_address1: " << arg.sensor_ip_address1; - os << ", "; - os << "configuration_counter: " << arg.configuration_counter; - os << ", "; - os << "status_longitudinal_velocity: " << arg.longitudinal_velocity_status; - os << ", "; - os << "status_longitudinal_acceleration: " << arg.longitudinal_acceleration_status; - os << ", "; - os << "status_lateral_acceleration: " << arg.lateral_acceleration_status; - os << ", "; - os << "status_yaw_rate: " << arg.yaw_rate_status; - os << ", "; - os << "status_steering_angle: " << arg.steering_angle_status; - os << ", "; - os << "status_driving_direction: " << arg.driving_direction_status; - os << ", "; - os << "characteristic_speed: " << arg.characteristic_speed_status; - os << ", "; - os << "radar_status: " << arg.radar_status; - os << ", "; - os << "temperature_status: " << arg.temperature_status; - os << ", "; - os << "voltage_status: " << arg.voltage_status; - os << ", "; - os << "blockage_status: " << arg.blockage_status; - - return os; - } -}; - -} // namespace nebula -#endif // CONTINENTAL_TYPES_HPP diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp new file mode 100644 index 000000000..b029ae40e --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/multi_continental_ars548_hw_interface.hpp @@ -0,0 +1,179 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H +#define NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H +// Have to define macros to silence warnings about deprecated headers being used by +// boost/property_tree/ in some versions of boost. +// See: https://github.com/boostorg/property_tree/issues/51 +#include +#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 +#define BOOST_BIND_GLOBAL_PLACEHOLDERS +#endif +#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#endif +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +/// @brief Hardware interface of the Continental ARS548 radar +class MultiContinentalARS548HwInterface : NebulaHwInterfaceBase +{ +private: + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; + std::vector> sensor_udp_drivers_; + std::shared_ptr sensor_configuration_; + std::unique_ptr nebula_packets_ptr_; + std::function buffer, const std::string & sensor_ip)> + nebula_packets_reception_callback_; + + std::mutex sensor_status_mutex_; + + SensorStatusPacket sensor_status_packet_{}; + FilterStatusPacket filter_status_{}; + std::unordered_map sensor_ip_to_frame_; + + std::shared_ptr parent_node_logger; + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM + /// @param bytes Target byte vector + void PrintDebug(const std::vector & bytes); + +public: + /// @brief Constructor + MultiContinentalARS548HwInterface(); + + /// @brief Process a new filter status packet + /// @param buffer The buffer containing the status packet + void ProcessFilterStatusPacket(const std::vector & buffer); + + /// @brief Process a new data packet + /// @param buffer The buffer containing the data packet + void ProcessDataPacket(const std::vector & buffer, const std::string & sensor_ip); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback( + const std::vector & buffer, const std::string & sensor_ip); + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart() final; + + /// @brief Function for stopping the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStop() final; + + /// @brief Printing sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) final; + + /// @brief Setting sensor configuration + /// @param sensor_configuration SensorConfiguration for this interface + /// @return Resulting status + Status SetSensorConfiguration( + std::shared_ptr sensor_configuration) final; + + /// @brief Registering callback for PandarScan + /// @param scan_callback Callback function + /// @return Resulting status + Status RegisterScanCallback( + std::function, const std::string &)> + scan_callback); + + /// @brief Set the current lateral acceleration + /// @param lateral_acceleration Current lateral acceleration + /// @return Resulting status + Status SetAccelerationLateralCog(float lateral_acceleration); + + /// @brief Set the current longitudinal acceleration + /// @param longitudinal_acceleration Current longitudinal acceleration + /// @return Resulting status + Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); + + /// @brief Set the characteristic speed + /// @param characteristic_speed Characteristic speed + /// @return Resulting status + Status SetCharacteristicSpeed(float characteristic_speed); + + /// @brief Set the current direction + /// @param direction Current driving direction + /// @return Resulting status + Status SetDrivingDirection(int direction); + + /// @brief Set the current steering angle + /// @param angle_rad Current steering angle in radians + /// @return Resulting status + Status SetSteeringAngleFrontAxle(float angle_rad); + + /// @brief Set the current vehicle velocity + /// @param velocity Current vehicle velocity + /// @return Resulting status + Status SetVelocityVehicle(float velocity); + + /// @brief Set the current yaw rate + /// @param yaw_rate Current yaw rate + /// @return Resulting status + Status SetYawRate(float yaw_rate); + + /// @brief Checking the current settings and changing the difference point + /// @return Resulting status + Status CheckAndSetConfig(); + + /// @brief Setting rclcpp::Logger + /// @param node Logger + void SetLogger(std::shared_ptr node); +}; +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula + +#endif // NEBULA_MULTI_CONTINENTAL_ARS548_HW_INTERFACE_H diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index ad0ce4c16..a1f646cac 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.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_HESAI_HW_INTERFACE_H #define NEBULA_HESAI_HW_INTERFACE_H // Have to define macros to silence warnings about deprecated headers being used by @@ -10,13 +24,13 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" #include @@ -29,6 +43,8 @@ #include #include +#include +#include namespace nebula { @@ -77,12 +93,15 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2 -const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0 -const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2 -const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) +const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2 +const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0 +const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2 +const int PTP_LOG_ANNOUNCE_INTERVAL = + 1; // Time interval between Announce messages, in units of log seconds (default: 1) +const int PTP_SYNC_INTERVAL = + 1; // Time interval between Sync messages, in units of log seconds (default: 1) +const int PTP_LOG_MIN_DELAY_INTERVAL = 0; // Minimum permitted mean time between Delay_Req + // messages, in units of log seconds (default: 0) const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; @@ -182,13 +201,13 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveCloudPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStart() final; + Status SensorInterfaceStart() final; /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStop() final; + Status SensorInterfaceStop() final; /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status @@ -376,8 +395,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) /// @param target_tcp_driver TcpDriver used /// @return Resulting status - Status syncGetInventory( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); + Status syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) /// @param ctx IO Context used /// @param callback Callback function for received HesaiInventory @@ -770,8 +788,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status GetLidarRange(bool with_run = true); - Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run); - Status SetClockSource(std::shared_ptr ctx, int clock_source, bool with_run); + Status SetClockSource( + std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, + bool with_run); + Status SetClockSource( + std::shared_ptr ctx, int clock_source, bool with_run); Status SetClockSource(int clock_source, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG @@ -916,27 +937,15 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); HesaiStatus SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); + HesaiStatus SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval); HesaiStatus SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle); + std::shared_ptr ctx, int enable, int angle); HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); - - /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string @@ -967,8 +976,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus CheckAndSetConfig(); /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel - /// @param model - /// @return + /// @param model + /// @return int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index cd45984af..38db8f0c6 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #pragma once // Have to define macros to silence warnings about deprecated headers being used by @@ -21,6 +37,10 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" +#include +#include +#include + namespace nebula { namespace drivers @@ -70,7 +90,7 @@ class RobosenseHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveCloudPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Callback function to receive the Info Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket @@ -78,7 +98,7 @@ class RobosenseHwInterface : NebulaHwInterfaceBase /// @brief Starting the interface that handles UDP streams for MSOP packets /// @return Resulting status - Status CloudInterfaceStart() final; + Status SensorInterfaceStart() final; /// @brief Starting the interface that handles UDP streams for DIFOP packets /// @return Resulting status @@ -86,7 +106,7 @@ class RobosenseHwInterface : NebulaHwInterfaceBase /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStop() final; + Status SensorInterfaceStop() final; /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index 6256c5e2b..c6c1311f4 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.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_VELODYNE_HW_INTERFACE_H #define NEBULA_VELODYNE_HW_INTERFACE_H @@ -26,6 +40,8 @@ #include #include +#include +#include namespace nebula { @@ -106,13 +122,13 @@ class VelodyneHwInterface : NebulaHwInterfaceBase /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveCloudPacketCallback(const std::vector & buffer) final; + void ReceiveSensorPacketCallback(const std::vector & buffer) final; /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStart() final; + Status SensorInterfaceStart() final; /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status CloudInterfaceStop() final; + Status SensorInterfaceStop() final; /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status 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 3fdb59280..82ca7a28a 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,9 +13,8 @@ // 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 "nebula_common/continental/continental_ars548.hpp" +#include +#include #include #include @@ -26,8 +25,8 @@ namespace drivers namespace continental_ars548 { ContinentalARS548HwInterface::ContinentalARS548HwInterface() -: cloud_io_context_{new ::drivers::common::IoContext(1)}, - sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*cloud_io_context_)}, +: sensor_io_context_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)}, nebula_packets_ptr_{std::make_unique()} { } @@ -49,7 +48,7 @@ Status ContinentalARS548HwInterface::SetSensorConfiguration( return Status::OK; } -Status ContinentalARS548HwInterface::CloudInterfaceStart() +Status ContinentalARS548HwInterface::SensorInterfaceStart() { try { sensor_udp_driver_->init_receiver( @@ -59,7 +58,7 @@ Status ContinentalARS548HwInterface::CloudInterfaceStart() sensor_udp_driver_->receiver()->open(); sensor_udp_driver_->receiver()->bind(); sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( - &ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender, this, + &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, std::placeholders::_1, std::placeholders::_2)); sensor_udp_driver_->init_sender( @@ -88,14 +87,14 @@ Status ContinentalARS548HwInterface::RegisterScanCallback( return Status::OK; } -void ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender( +void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( const std::vector & buffer, const std::string & sender_ip) { if (sender_ip == sensor_configuration_->sensor_ip) { - ReceiveCloudPacketCallback(buffer); + ReceiveSensorPacketCallback(buffer); } } -void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); @@ -115,7 +114,7 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< PrintError("SensorStatus message with invalid size"); return; } - ProcessSensorStatusPacket(buffer); + ProcessDataPacket(buffer); } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { if ( buffer.size() != FILTER_STATUS_UDP_PAYLOAD || @@ -146,191 +145,6 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< } } -void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) -{ - std::lock_guard l(sensor_status_mutex_); - - std::memcpy(&sensor_status_packet_, buffer.data(), sizeof(SensorStatusPacket)); - - radar_status_.timestamp_nanoseconds = sensor_status_packet_.stamp.timestamp_nanoseconds.value(); - radar_status_.timestamp_seconds = sensor_status_packet_.stamp.timestamp_seconds.value(); - - if (sensor_status_packet_.stamp.timestamp_sync_status == 1) { - radar_status_.timestamp_sync_status = "SYNC_OK"; - } else if (sensor_status_packet_.stamp.timestamp_sync_status == 2) { - radar_status_.timestamp_sync_status = "NEVER_SYNC"; - } else if (sensor_status_packet_.stamp.timestamp_sync_status == 3) { - radar_status_.timestamp_sync_status = "SYNC_LOST"; - } else { - radar_status_.timestamp_sync_status = "INVALID_VALUE"; - } - - radar_status_.sw_version_major = sensor_status_packet_.sw_version_major; - radar_status_.sw_version_minor = sensor_status_packet_.sw_version_minor; - radar_status_.sw_version_patch = sensor_status_packet_.sw_version_patch; - - radar_status_.plug_orientation = sensor_status_packet_.status.plug_orientation == 0 ? "PLUG_RIGHT" - : sensor_status_packet_.status.plug_orientation == 1 - ? "PLUG_LEFT" - : "INVALID_VALUE"; - - radar_status_.max_distance = sensor_status_packet_.status.maximum_distance.value(); - - radar_status_.longitudinal = sensor_status_packet_.status.longitudinal.value(); - radar_status_.lateral = sensor_status_packet_.status.lateral.value(); - radar_status_.vertical = sensor_status_packet_.status.vertical.value(); - radar_status_.yaw = sensor_status_packet_.status.yaw.value(); - - radar_status_.pitch = sensor_status_packet_.status.pitch.value(); - ; - radar_status_.length = sensor_status_packet_.status.length.value(); - ; - radar_status_.width = sensor_status_packet_.status.width.value(); - radar_status_.height = sensor_status_packet_.status.height.value(); - radar_status_.wheel_base = sensor_status_packet_.status.wheelbase.value(); - - if (sensor_status_packet_.status.frequency_slot == 0) { - radar_status_.frequency_slot = "0:Low (76.23 GHz)"; - } else if (sensor_status_packet_.status.frequency_slot == 1) { - radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; - } else if (sensor_status_packet_.status.frequency_slot == 2) { - radar_status_.frequency_slot = "2:High (76.73 GHz)"; - } else { - radar_status_.frequency_slot = "INVALID VALUE"; - } - - radar_status_.cycle_time = sensor_status_packet_.status.cycle_time; - radar_status_.time_slot = sensor_status_packet_.status.time_slot; - - radar_status_.hcc = sensor_status_packet_.status.hcc == 1 ? "Worldwide" - : sensor_status_packet_.status.hcc == 2 - ? "Japan" - : ("INVALID VALUE=" + std::to_string(sensor_status_packet_.status.hcc)); - - radar_status_.power_save_standstill = - sensor_status_packet_.status.powersave_standstill == 0 ? "Off" - : sensor_status_packet_.status.powersave_standstill == 1 ? "On" - : "INVALID VALUE"; - - std::stringstream ss0, ss1; - ss0 << std::to_string(sensor_status_packet_.status.sensor_ip_address00) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address01) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address02) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address03); - radar_status_.sensor_ip_address0 = ss0.str(); - - ss1 << std::to_string(sensor_status_packet_.status.sensor_ip_address10) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address11) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address12) << "." - << std::to_string(sensor_status_packet_.status.sensor_ip_address13); - radar_status_.sensor_ip_address1 = ss1.str(); - - radar_status_.configuration_counter = sensor_status_packet_.configuration_counter; - - radar_status_.longitudinal_velocity_status = - sensor_status_packet_.longitudinal_velocity_status == 0 ? "VDY_OK" - : sensor_status_packet_.longitudinal_velocity_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.longitudinal_acceleration_status = - sensor_status_packet_.longitudinal_acceleration_status == 0 ? "VDY_OK" - : sensor_status_packet_.longitudinal_acceleration_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.lateral_acceleration_status = - sensor_status_packet_.lateral_acceleration_status == 0 ? "VDY_OK" - : sensor_status_packet_.lateral_acceleration_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.yaw_rate_status = sensor_status_packet_.yaw_rate_status == 0 ? "VDY_OK" - : sensor_status_packet_.yaw_rate_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.steering_angle_status = sensor_status_packet_.steering_angle_status == 0 ? "VDY_OK" - : sensor_status_packet_.steering_angle_status == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.driving_direction_status = - sensor_status_packet_.driving_direction_status == 0 ? "VDY_OK" - : sensor_status_packet_.driving_direction_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.characteristic_speed_status = - sensor_status_packet_.characteristic_speed_status == 0 ? "VDY_OK" - : sensor_status_packet_.characteristic_speed_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - if (sensor_status_packet_.radar_status == 0) { - radar_status_.radar_status = "STATE_INIT"; - } else if (sensor_status_packet_.radar_status == 1) { - radar_status_.radar_status = "STATE_OK"; - } else if (sensor_status_packet_.radar_status == 2) { - radar_status_.radar_status = "STATE_INVALID"; - } else { - radar_status_.radar_status = "INVALID VALUE"; - } - - if (sensor_status_packet_.voltage_status == 0) { - radar_status_.voltage_status = "Ok"; - } - if (sensor_status_packet_.voltage_status & 0x01) { - radar_status_.voltage_status += "Current under voltage"; - } - if (sensor_status_packet_.voltage_status & 0x02) { - radar_status_.voltage_status = "Past under voltage"; - } - if (sensor_status_packet_.voltage_status & 0x03) { - radar_status_.voltage_status = "Current over voltage"; - } - if (sensor_status_packet_.voltage_status & 0x04) { - radar_status_.voltage_status = "Past over voltage"; - } - - if (sensor_status_packet_.temperature_status == 0) { - radar_status_.temperature_status = "Ok"; - } - if (sensor_status_packet_.temperature_status & 0x01) { - radar_status_.temperature_status += "Current under temperature"; - } - if (sensor_status_packet_.temperature_status & 0x02) { - radar_status_.temperature_status += "Past under temperature"; - } - if (sensor_status_packet_.temperature_status & 0x03) { - radar_status_.temperature_status += "Current over temperature"; - } - if (sensor_status_packet_.temperature_status & 0x04) { - radar_status_.temperature_status += "Past over temperature"; - } - - const uint8_t & blockage_status0 = sensor_status_packet_.blockage_status & 0x0f; - const uint8_t & blockage_status1 = (sensor_status_packet_.blockage_status & 0xf0) >> 4; - - if (blockage_status0 == 0) { - radar_status_.blockage_status = "Blind"; - } else if (blockage_status0 == 1) { - radar_status_.blockage_status = "High"; - } else if (blockage_status0 == 2) { - radar_status_.blockage_status = "Mid"; - } else if (blockage_status0 == 3) { - radar_status_.blockage_status = "Low"; - } else if (blockage_status0 == 4) { - radar_status_.blockage_status = "None"; - } else { - radar_status_.blockage_status = "INVALID VALUE"; - } - - if (blockage_status1 == 0) { - radar_status_.blockage_status += ". Self test failed"; - } else if (blockage_status1 == 1) { - radar_status_.blockage_status += ". Self test passed"; - } else if (blockage_status1 == 2) { - radar_status_.blockage_status += ". Self test ongoing"; - } else { - radar_status_.blockage_status += ". Invalid self test"; - } -} - void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) { assert(buffer.size() == sizeof(FilterStatusPacket)); @@ -357,7 +171,7 @@ void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector nebula_packets_ptr_ = std::make_unique(); } -Status ContinentalARS548HwInterface::CloudInterfaceStop() +Status ContinentalARS548HwInterface::SensorInterfaceStop() { return Status::ERROR_1; } @@ -466,6 +280,7 @@ Status ContinentalARS548HwInterface::SetRadarParameters( configuration_packet.configuration.maximum_distance = maximum_distance; configuration_packet.configuration.frequency_slot = frequency_slot; configuration_packet.configuration.cycle_time = cycle_time; + configuration_packet.configuration.time_slot = time_slot; configuration_packet.configuration.hcc = hcc; configuration_packet.configuration.powersave_standstill = power_save_standstill; configuration_packet.new_radar_parameters = 1; @@ -476,6 +291,7 @@ Status ContinentalARS548HwInterface::SetRadarParameters( PrintInfo("maximum_distance = " + std::to_string(maximum_distance)); PrintInfo("frequency_slot = " + std::to_string(frequency_slot)); PrintInfo("cycle_time = " + std::to_string(cycle_time)); + PrintInfo("time_slot = " + std::to_string(time_slot)); PrintInfo("hcc = " + std::to_string(hcc)); PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); @@ -643,7 +459,7 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) constexpr uint8_t STEERING_ANGLE_LENGTH = 32; const int STEERING_ANGLE_UDP_LENGTH = 40; - if (angle_rad < -M_PI_2 || angle_rad > M_PI_2) { + if (angle_rad < -90.f || angle_rad > 90.f) { PrintError("Invalid angle_rad value"); return Status::ERROR_1; } @@ -664,8 +480,13 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::OK; } -Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) +Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) { + if (velocity_kmh < 0.f || velocity_kmh > 350.f) { + PrintError("Invalid velocity value"); + return Status::ERROR_1; + } + constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; @@ -676,7 +497,7 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; - steering_angle_front_axle_packet.velocity_vehicle = velocity; + steering_angle_front_axle_packet.velocity_vehicle = velocity_kmh; std::vector send_vector(sizeof(VelocityVehiclePacket)); std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); @@ -688,6 +509,11 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) { + if (yaw_rate < -163.83 || yaw_rate > 163.83) { + PrintError("Invalid yaw rate value"); + return Status::ERROR_1; + } + constexpr uint16_t YAW_RATE_SERVICE_ID = 0; constexpr uint16_t YAW_RATE_METHOD_ID = 326; constexpr uint8_t YAW_RATE_LENGTH = 32; @@ -708,12 +534,6 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } -ContinentalARS548Status ContinentalARS548HwInterface::GetRadarStatus() -{ - std::lock_guard l(sensor_status_mutex_); - return radar_status_; -} - void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) { parent_node_logger = logger; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp new file mode 100644 index 000000000..58cd25f09 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp @@ -0,0 +1,453 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// + +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +MultiContinentalARS548HwInterface::MultiContinentalARS548HwInterface() +: sensor_io_context_{new ::drivers::common::IoContext(1)}, + nebula_packets_ptr_{std::make_unique()} +{ +} + +Status MultiContinentalARS548HwInterface::SetSensorConfiguration( + std::shared_ptr sensor_configuration) +{ + Status status = Status::OK; + + try { + sensor_configuration_ = + std::static_pointer_cast(sensor_configuration); + } catch (const std::exception & ex) { + status = Status::SENSOR_CONFIG_ERROR; + std::cerr << status << std::endl; + return status; + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SensorInterfaceStart() +{ + for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_->sensor_ips.size(); + sensor_id++) { + auto udp_driver = std::make_unique<::drivers::udp_driver::UdpDriver>(*sensor_io_context_); + sensor_ip_to_frame_[sensor_configuration_->sensor_ips[sensor_id]] = + sensor_configuration_->frame_ids[sensor_id]; + + try { + if (sensor_id == 0) { + udp_driver->init_receiver( + sensor_configuration_->multicast_ip, sensor_configuration_->data_port, + sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); + udp_driver->receiver()->setMulticast(true); + udp_driver->receiver()->open(); + udp_driver->receiver()->bind(); + udp_driver->receiver()->asyncReceiveWithSender(std::bind( + &MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + udp_driver->init_sender( + sensor_configuration_->sensor_ips[sensor_id], + sensor_configuration_->configuration_sensor_port, sensor_configuration_->host_ip, + sensor_configuration_->configuration_host_port); + + udp_driver->sender()->open(); + udp_driver->sender()->bind(); + + if (!udp_driver->sender()->isOpen()) { + return Status::ERROR_1; + } + } catch (const std::exception & ex) { + Status status = Status::UDP_CONNECTION_ERROR; + std::cerr << status << sensor_configuration_->sensor_ip << "," + << sensor_configuration_->data_port << std::endl; + return status; + } + + sensor_udp_drivers_.emplace_back(std::move(udp_driver)); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::RegisterScanCallback( + std::function, const std::string &)> + callback) +{ + nebula_packets_reception_callback_ = std::move(callback); + return Status::OK; +} + +void MultiContinentalARS548HwInterface::ReceiveSensorPacketCallback( + const std::vector & buffer, const std::string & sender_ip) +{ + if (buffer.size() < sizeof(HeaderPacket)) { + PrintError("Unrecognized packet. Too short"); + return; + } + + HeaderPacket header_packet{}; + std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); + + if (header_packet.service_id.value() != 0) { + PrintError("Invalid service id"); + return; + } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { + if ( + buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || + header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { + PrintError("SensorStatus message with invalid size"); + return; + } + ProcessDataPacket(buffer, sender_ip); + } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { + if ( + buffer.size() != FILTER_STATUS_UDP_PAYLOAD || + header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { + PrintError("FilterStatus message with invalid size"); + return; + } + + ProcessFilterStatusPacket(buffer); + } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + buffer.size() != DETECTION_LIST_UDP_PAYLOAD || + header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { + PrintError("DetectionList message with invalid size"); + return; + } + + ProcessDataPacket(buffer, sender_ip); + } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { + if ( + buffer.size() != OBJECT_LIST_UDP_PAYLOAD || + header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { + PrintError("ObjectList message with invalid size"); + return; + } + + ProcessDataPacket(buffer, sender_ip); + } +} + +void MultiContinentalARS548HwInterface::ProcessFilterStatusPacket( + const std::vector & buffer) +{ + assert(buffer.size() == sizeof(FilterStatusPacket)); + std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); +} + +void MultiContinentalARS548HwInterface::ProcessDataPacket( + const std::vector & buffer, const std::string & sensor_ip) +{ + nebula_msgs::msg::NebulaPacket nebula_packet; + nebula_packet.data = buffer; + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + std::chrono::duration_cast(now.time_since_epoch()).count(); + nebula_packet.stamp.sec = static_cast(now_secs); + nebula_packet.stamp.nanosec = + static_cast((now_nanosecs / 1000000000.0 - static_cast(now_secs)) * 1000000000); + nebula_packets_ptr_->packets.emplace_back(nebula_packet); + + nebula_packets_ptr_->header.stamp = nebula_packets_ptr_->packets.front().stamp; + nebula_packets_ptr_->header.frame_id = sensor_configuration_->frame_id; + + nebula_packets_reception_callback_(std::move(nebula_packets_ptr_), sensor_ip); + nebula_packets_ptr_ = std::make_unique(); +} + +Status MultiContinentalARS548HwInterface::SensorInterfaceStop() +{ + return Status::ERROR_1; +} + +Status MultiContinentalARS548HwInterface::GetSensorConfiguration( + SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + PrintDebug(ss.str()); + return Status::ERROR_1; +} + +Status MultiContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) +{ + constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; + constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; + constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; + const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; + + if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { + PrintError("Invalid lateral_acceleration value"); + return Status::ERROR_1; + } + + AccelerationLateralCoGPacket acceleration_lateral_cog{}; + static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); + acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; + acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; + acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; + acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; + + std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); + std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetAccelerationLongitudinalCog( + float longitudinal_acceleration) +{ + constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; + constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; + constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; + const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; + + if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { + PrintError("Invalid longitudinal_acceleration value"); + return Status::ERROR_1; + } + + AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; + static_assert( + sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); + acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; + acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; + acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; + acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; + + std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); + std::memcpy( + send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) +{ + constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; + constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; + constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; + const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; + + if (characteristic_speed < 0.f || characteristic_speed > 255.f) { + PrintError("Invalid characteristic_speed value"); + return Status::ERROR_1; + } + + CharacteristicSpeedPacket characteristic_speed_packet{}; + static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); + characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; + characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; + characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; + characteristic_speed_packet.characteristic_speed = characteristic_speed; + + std::vector send_vector(sizeof(CharacteristicSpeedPacket)); + std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetDrivingDirection(int direction) +{ + constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; + constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; + constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; + constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; + constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; + constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; + const int DRIVING_DIRECTION_UDP_LENGTH = 30; + + DrivingDirectionPacket driving_direction_packet{}; + static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); + driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; + driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; + driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; + + if (direction == 0) { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; + } else if (direction > 0) { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; + } else { + driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; + } + + std::vector send_vector(sizeof(DrivingDirectionPacket)); + std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) +{ + constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; + constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; + constexpr uint8_t STEERING_ANGLE_LENGTH = 32; + const int STEERING_ANGLE_UDP_LENGTH = 40; + + if (angle_rad < -90.f || angle_rad > 90.f) { + PrintError("Invalid angle_rad value"); + return Status::ERROR_1; + } + + SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); + steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; + steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; + steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; + steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; + + std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); + std::memcpy( + send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) +{ + if (velocity_kmh < 0.f || velocity_kmh > 350.f) { + PrintError("Invalid velocity value"); + return Status::ERROR_1; + } + + constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; + constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; + constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; + const int VELOCITY_VEHICLE_UDP_SIZE = 36; + + VelocityVehiclePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); + steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; + steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; + steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; + steering_angle_front_axle_packet.velocity_vehicle = velocity_kmh; + + std::vector send_vector(sizeof(VelocityVehiclePacket)); + std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +Status MultiContinentalARS548HwInterface::SetYawRate(float yaw_rate) +{ + if (yaw_rate < -163.83 || yaw_rate > 163.83) { + PrintError("Invalid yaw rate value"); + return Status::ERROR_1; + } + + constexpr uint16_t YAW_RATE_SERVICE_ID = 0; + constexpr uint16_t YAW_RATE_METHOD_ID = 326; + constexpr uint8_t YAW_RATE_LENGTH = 32; + const int YAW_RATE_UDP_SIZE = 40; + + YawRatePacket yaw_rate_packet{}; + static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); + yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; + yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; + yaw_rate_packet.header.length = YAW_RATE_LENGTH; + yaw_rate_packet.yaw_rate = yaw_rate; + + std::vector send_vector(sizeof(YawRatePacket)); + std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); + + for (auto & udp_driver : sensor_udp_drivers_) { + udp_driver->sender()->asyncSend(send_vector); + } + + return Status::OK; +} + +void MultiContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger = logger; +} + +void MultiContinentalARS548HwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger) { + RCLCPP_INFO_STREAM((*parent_node_logger), info); + } else { + std::cout << info << std::endl; + } +} + +void MultiContinentalARS548HwInterface::PrintError(std::string error) +{ + if (parent_node_logger) { + RCLCPP_ERROR_STREAM((*parent_node_logger), error); + } else { + std::cerr << error << std::endl; + } +} + +void MultiContinentalARS548HwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger) { + RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + } else { + std::cout << debug << std::endl; + } +} + +void MultiContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) +{ + std::stringstream ss; + for (const auto & b : bytes) { + ss << static_cast(b) << ", "; + } + ss << std::endl; + PrintDebug(ss.str()); +} + +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index f5815fc40..96318ad10 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -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. + #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" //#define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -26,13 +40,11 @@ HesaiHwInterface::~HesaiHwInterface() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; #endif - if(tcp_driver_) - { + if (tcp_driver_) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_ is available" << std::endl; #endif - if(tcp_driver_ && tcp_driver_->isOpen()) - { + if (tcp_driver_ && tcp_driver_->isOpen()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_->close();" << std::endl; #endif @@ -45,13 +57,11 @@ HesaiHwInterface::~HesaiHwInterface() std::cout << ".......................ed: if(tcp_driver_)" << std::endl; #endif } - if(tcp_driver_s_) - { + if (tcp_driver_s_) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_s_ is available" << std::endl; #endif - if(tcp_driver_s_->isOpen()) - { + if (tcp_driver_s_->isOpen()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_s_->close();" << std::endl; #endif @@ -132,7 +142,7 @@ Status HesaiHwInterface::SetSensorConfiguration( return Status::OK; } -Status HesaiHwInterface::CloudInterfaceStart() +Status HesaiHwInterface::SensorInterfaceStart() { try { std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; @@ -151,7 +161,7 @@ Status HesaiHwInterface::CloudInterfaceStart() #endif cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + std::bind(&HesaiHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE PrintError("async receive set"); #endif @@ -171,7 +181,7 @@ Status HesaiHwInterface::RegisterScanCallback( return Status::OK; } -void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); if (!is_valid_packet_(buffer.size())) { @@ -221,7 +231,10 @@ void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & b } } } -Status HesaiHwInterface::CloudInterfaceStop() { return Status::ERROR_1; } +Status HesaiHwInterface::SensorInterfaceStop() +{ + return Status::ERROR_1; +} Status HesaiHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) { @@ -255,9 +268,9 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) #endif if (!tcp_driver_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_->open()" << std::endl; + std::cout << "!tcp_driver_->open()" << std::endl; #endif -// tcp_driver_->close(); + // tcp_driver_->close(); tcp_driver_->closeSync(); return Status::ERROR_1; } @@ -268,9 +281,9 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) PandarTcpCommandPort); if (!tcp_driver_s_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_s_->open()" << std::endl; + std::cout << "!tcp_driver_s_->open()" << std::endl; #endif -// tcp_driver_s_->close(); + // tcp_driver_s_->close(); tcp_driver_s_->closeSync(); return Status::ERROR_1; } @@ -278,11 +291,11 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() { +Status HesaiHwInterface::FinalizeTcpDriver() +{ try { tcp_driver_->close(); - } - catch(std::exception &e) { + } catch (std::exception & e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } @@ -320,7 +333,7 @@ Status HesaiHwInterface::syncGetLidarCalibration( target_tcp_driver->syncSendReceiveHeaderPayload( buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { + [this]([[maybe_unused]] const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -424,7 +437,7 @@ Status HesaiHwInterface::GetLidarCalibration( target_tcp_driver->asyncSendReceiveHeaderPayload( buf_vec, - [this]([[maybe_unused]]const std::vector & received_bytes) { + [this]([[maybe_unused]] const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -1049,7 +1062,11 @@ Status HesaiHwInterface::syncGetInventory( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback) { - std::cout << "Status HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback)" << std::endl; + std::cout + << "Status " + "HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> " + "target_tcp_driver, std::function callback)" + << std::endl; std::vector buf_vec; int len = 0; buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); @@ -1066,13 +1083,13 @@ Status HesaiHwInterface::syncGetInventory( } // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver){ + if (!target_tcp_driver) { PrintError("!target_tcp_driver"); return Status::ERROR_1; } // It doesn't work even when the sensor is not connected... - if(!target_tcp_driver->isOpen()){ + if (!target_tcp_driver->isOpen()) { PrintError("!target_tcp_driver->isOpen()"); return Status::ERROR_1; } @@ -1090,7 +1107,8 @@ Status HesaiHwInterface::syncGetInventory( #endif PrintDebug(received_bytes); }, - [this, target_tcp_driver, callback]([[maybe_unused]]const std::vector & received_bytes) { + [this, target_tcp_driver, + callback]([[maybe_unused]] const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -1151,8 +1169,8 @@ Status HesaiHwInterface::syncGetInventory( Status HesaiHwInterface::syncGetInventory( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) { - return syncGetInventory(target_tcp_driver, - [this](HesaiInventory & result) { std::cout << result << std::endl; }); + return syncGetInventory( + target_tcp_driver, [this](HesaiInventory & result) { std::cout << result << std::endl; }); } Status HesaiHwInterface::syncGetInventory( std::shared_ptr ctx, @@ -1169,9 +1187,7 @@ Status HesaiHwInterface::syncGetInventory(std::shared_ptr callback) { return syncGetInventory( - tcp_driver_, [this, callback](HesaiInventory & result) { - callback(result); - }); + tcp_driver_, [this, callback](HesaiInventory & result) { callback(result); }); } Status HesaiHwInterface::GetInventory( @@ -2723,9 +2739,15 @@ Status HesaiHwInterface::GetLidarMonitor(bool with_run) [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); } -void HesaiHwInterface::IOContextRun() { m_owned_ctx->run(); } +void HesaiHwInterface::IOContextRun() +{ + m_owned_ctx->run(); +} -std::shared_ptr HesaiHwInterface::GetIOContext() { return m_owned_ctx; } +std::shared_ptr HesaiHwInterface::GetIOContext() +{ + return m_owned_ctx; +} HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::shared_ptr ctx, @@ -2755,7 +2777,10 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( return st; } -void HesaiHwInterface::str_cb(const std::string & str) { PrintInfo(str); } +void HesaiHwInterface::str_cb(const std::string & str) +{ + PrintInfo(str); +} HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( std::shared_ptr ctx, uint16_t rpm) @@ -2794,13 +2819,8 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( - std::shared_ptr ctx, - int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval) + std::shared_ptr ctx, int profile, int domain, int network, + int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); @@ -2808,53 +2828,47 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( return st; } - auto response = hcd->get( - (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ - "\"Profile\": %d," \ - "\"Domain\": %d," \ - "\"Network\": %d," \ - "\"LogAnnounceInterval\": %d," \ - "\"LogSyncInterval\": %d," \ - "\"LogMinDelayReqInterval\": %d," \ - "\"tsn_switch\": %d" \ - "}") - % profile % domain % network % logAnnounceInterval % logSyncInterval % logMinDelayReqInterval % 0 - ).str()); + auto response = + hcd->get((boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" + "\"Profile\": %d," + "\"Domain\": %d," + "\"Network\": %d," + "\"LogAnnounceInterval\": %d," + "\"LogSyncInterval\": %d," + "\"LogMinDelayReqInterval\": %d," + "\"tsn_switch\": %d" + "}") % + profile % domain % network % logAnnounceInterval % logSyncInterval % + logMinDelayReqInterval % 0) + .str()); ctx->run(); PrintInfo(response); return Status::OK; } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, - int domain, - int network, - int logAnnounceInterval, - int logSyncInterval, - int logMinDelayReqInterval) +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( + int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, + int logMinDelayReqInterval) { - return SetPtpConfigSyncHttp(std::make_shared(), - profile, - domain, - network, - logAnnounceInterval, - logSyncInterval, - logMinDelayReqInterval); + return SetPtpConfigSyncHttp( + std::make_shared(), profile, domain, network, logAnnounceInterval, + logSyncInterval, logMinDelayReqInterval); } HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( - std::shared_ptr ctx, - int enable, - int angle) + std::shared_ptr ctx, int enable, int angle) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { return st; } - auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ - "\"sync\": %d," \ - "\"syncAngle\": %d" \ - "}") % enable % angle).str(); + auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" + "\"sync\": %d," + "\"syncAngle\": %d" + "}") % + enable % angle) + .str(); PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); @@ -2864,9 +2878,7 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) { - return SetSyncAngleSyncHttp(std::make_shared(), - enable, - angle); + return SetSyncAngleSyncHttp(std::make_shared(), enable, angle); } HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( @@ -2916,7 +2928,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); + PrintError( + "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " + "mode."); return_mode_int = 2; } SetReturnMode(return_mode_int); @@ -2977,7 +2991,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( t.join(); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){ + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -2989,9 +3003,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { - SetSyncAngle(sync_flg, scan_phase); - }); + std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); t.join(); } @@ -2999,28 +3011,18 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP"); - SetPtpConfig(PTP_PROFILE, - PTP_DOMAIN_ID, - PTP_NETWORK_TRANSPORT, - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL - ); + SetPtpConfig( + PTP_PROFILE, PTP_DOMAIN_ID, PTP_NETWORK_TRANSPORT, PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); }); t.join(); - } - else { //AT128 only supports PTP setup via HTTP + } else { // AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, - static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP via HTTP"); - SetPtpConfigSyncHttp(PTP_PROFILE, - PTP_DOMAIN_ID, - PTP_NETWORK_TRANSPORT, - PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, - PTP_LOG_MIN_DELAY_INTERVAL); - + SetPtpConfigSyncHttp( + PTP_PROFILE, PTP_DOMAIN_ID, PTP_NETWORK_TRANSPORT, PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -3177,13 +3179,12 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() */ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) { - switch (model) { case SensorModel::HESAI_PANDAR40P: return 0; case SensorModel::HESAI_PANDAR64: return 2; - case SensorModel::HESAI_PANDARQT64://check required + case SensorModel::HESAI_PANDARQT64: // check required return 15; case SensorModel::HESAI_PANDAR40M: return 17; @@ -3193,9 +3194,9 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return 32; case SensorModel::HESAI_PANDARXT32M: return 38; - case SensorModel::HESAI_PANDAR128_E3X://check required + case SensorModel::HESAI_PANDAR128_E3X: // check required return 40; - case SensorModel::HESAI_PANDAR128_E4X://check required + case SensorModel::HESAI_PANDAR128_E4X: // check required return 40; case SensorModel::HESAI_PANDARAT128: return 48; @@ -3204,8 +3205,14 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return -1; } } -void HesaiHwInterface::SetTargetModel(int model) { target_model_no = model; } -void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) { target_model_no = NebulaModelToHesaiModelNo(model); } +void HesaiHwInterface::SetTargetModel(int model) +{ + target_model_no = model; +} +void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) +{ + target_model_no = NebulaModelToHesaiModelNo(model); +} bool HesaiHwInterface::UseHttpSetSpinRate(int model) { @@ -3245,7 +3252,10 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) break; } } -bool HesaiHwInterface::UseHttpSetSpinRate() { return UseHttpSetSpinRate(target_model_no); } +bool HesaiHwInterface::UseHttpSetSpinRate() +{ + return UseHttpSetSpinRate(target_model_no); +} bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) { switch (model) { @@ -3284,17 +3294,20 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() { return UseHttpGetLidarMonitor(target_model_no); } +bool HesaiHwInterface::UseHttpGetLidarMonitor() +{ + return UseHttpGetLidarMonitor(target_model_no); +} bool HesaiHwInterface::CheckLock( std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name) { if (wl) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE -// std::chrono::time_point start_time = std::chrono::steady_clock::now(); + // std::chrono::time_point start_time = std::chrono::steady_clock::now(); std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); std::time_t stt = std::chrono::system_clock::to_time_t(start_time); - const std::tm* system_local_time = std::localtime(&stt); + const std::tm * system_local_time = std::localtime(&stt); std::cout << "try_lock_for: start at " << std::put_time(system_local_time, "%c") << std::endl; /* std::chrono::system_clock::time_point test_time = std::chrono::system_clock::now(); @@ -3302,18 +3315,21 @@ bool HesaiHwInterface::CheckLock( const std::tm* local_time = std::localtime(&tst); std::cerr << "try_lock_for: test at " << std::put_time(local_time, "%c") << std::endl; auto dur_test = test_time - start_time; - std::cerr << "test: " << name << " : " << std::chrono::duration_cast(dur_test).count() << std::endl; + std::cerr << "test: " << name << " : " << + std::chrono::duration_cast(dur_test).count() << std::endl; */ #endif if (!tm.try_lock_for(std::chrono::milliseconds(timeout_))) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" << std::endl; - std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); - std::time_t edt = std::chrono::system_clock::to_time_t(end_time); - const std::tm* end_local_time = std::localtime(&edt); - std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; - auto dur = end_time - start_time; - std::cerr << "timeout: " << name << " : " << std::chrono::duration_cast(dur).count() << std::endl; + std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" + << std::endl; + std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); + std::time_t edt = std::chrono::system_clock::to_time_t(end_time); + const std::tm * end_local_time = std::localtime(&edt); + std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; + auto dur = end_time - start_time; + std::cerr << "timeout: " << name << " : " + << std::chrono::duration_cast(dur).count() << std::endl; #endif PrintDebug("timeout: " + name); fail_cnt++; @@ -3327,8 +3343,7 @@ bool HesaiHwInterface::CheckLock( tcp_driver_s_->closeSync(); tcp_driver_s_->open(); } - } - else { + } else { return true; } return false; diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index 8332933e0..05a3bdb6a 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -1,3 +1,19 @@ +// Copyright 2023 LeoDrive. +// +// 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. + +// Developed by LeoDrive, 2023 + #include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" namespace nebula { @@ -12,7 +28,7 @@ RobosenseHwInterface::RobosenseHwInterface() { } -void RobosenseHwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +void RobosenseHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { int scan_phase = static_cast(sensor_configuration_->scan_phase * 100.0); if (!is_valid_packet_(buffer.size())) { @@ -99,7 +115,7 @@ void RobosenseHwInterface::ReceiveInfoPacketCallback(const std::vector } } -Status RobosenseHwInterface::CloudInterfaceStart() +Status RobosenseHwInterface::SensorInterfaceStart() { try { std::cout << "Starting UDP server for data packets on: " << *sensor_configuration_ << std::endl; @@ -109,7 +125,7 @@ Status RobosenseHwInterface::CloudInterfaceStart() cloud_udp_driver_->receiver()->bind(); cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&RobosenseHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + std::bind(&RobosenseHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -133,7 +149,6 @@ Status RobosenseHwInterface::InfoInterfaceStart() info_udp_driver_->receiver()->asyncReceive( std::bind(&RobosenseHwInterface::ReceiveInfoPacketCallback, this, std::placeholders::_1)); - } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -145,7 +160,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() return Status::OK; } -Status RobosenseHwInterface::CloudInterfaceStop() +Status RobosenseHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } @@ -238,4 +253,4 @@ void RobosenseHwInterface::SetLogger(std::shared_ptr logger) } } // namespace drivers -} // namespace nebula \ No newline at end of file +} // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index f9df2840f..30f6a493a 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -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. + #include "nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp" namespace nebula @@ -36,7 +50,7 @@ Status VelodyneHwInterface::SetSensorConfiguration( return rt; } -Status VelodyneHwInterface::CloudInterfaceStart() +Status VelodyneHwInterface::SensorInterfaceStart() { try { cloud_udp_driver_->init_receiver( @@ -44,7 +58,7 @@ Status VelodyneHwInterface::CloudInterfaceStart() cloud_udp_driver_->receiver()->open(); cloud_udp_driver_->receiver()->bind(); cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&VelodyneHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1)); + std::bind(&VelodyneHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -61,7 +75,7 @@ Status VelodyneHwInterface::RegisterScanCallback( return Status::OK; } -void VelodyneHwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) +void VelodyneHwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) { // Process current packet const uint32_t buffer_size = buffer.size(); @@ -99,7 +113,7 @@ void VelodyneHwInterface::ReceiveCloudPacketCallback(const std::vector } prev_packet_first_azm_phased_ = packet_first_azm_phased_; } -Status VelodyneHwInterface::CloudInterfaceStop() +Status VelodyneHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 31ff306e8..dd81e1ac0 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -133,6 +133,14 @@ rclcpp_components_register_node(continental_ars548_hw_ros_wrapper EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node ) +ament_auto_add_library(multi_continental_ars548_hw_ros_wrapper SHARED + src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(multi_continental_ars548_hw_ros_wrapper + PLUGIN "MultiContinentalARS548HwInterfaceRosWrapper" + EXECUTABLE multi_continental_ars548_hw_interface_ros_wrapper_node + ) + # DriverDecoder ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -142,7 +150,6 @@ rclcpp_components_register_node(continental_ars548_driver_ros_wrapper EXECUTABLE continental_ars548_driver_ros_wrapper_node ) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp index 8f387226c..980e2d627 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.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_HW_INTERFACE_WRAPPER_BASE_H #define NEBULA_HW_INTERFACE_WRAPPER_BASE_H @@ -23,7 +37,7 @@ class NebulaHwInterfaceWrapperBase NebulaHwInterfaceWrapperBase(const NebulaHwInterfaceWrapperBase & c) = delete; NebulaHwInterfaceWrapperBase & operator=(const NebulaHwInterfaceWrapperBase & c) = delete; - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status virtual Status StreamStart() = 0; diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp index e3636dd0f..059388639 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp @@ -16,9 +16,7 @@ #define NEBULA_ContinentalARS548DriverRosWrapper_H #include -#include #include -#include #include #include #include @@ -31,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -61,6 +60,7 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive rclcpp::Publisher::SharedPtr scan_raw_pub_; rclcpp::Publisher::SharedPtr objects_raw_pub_; rclcpp::Publisher::SharedPtr objects_markers_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; std::unordered_set previous_ids_; @@ -76,7 +76,8 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive {{0.0, -1.0}}, {{0.0, 0.0}}}}; - std::shared_ptr sensor_cfg_ptr_; + std::shared_ptr + sensor_cfg_ptr_; drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; @@ -91,7 +92,8 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @param calibration_configuration Output of CalibrationConfiguration /// @param correction_configuration Output of CorrectionConfiguration (for AT) /// @return Resulting status - Status GetParameters(drivers::ContinentalARS548SensorConfiguration & sensor_configuration); + Status GetParameters( + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds @@ -111,6 +113,11 @@ class ContinentalARS548DriverRosWrapper final : public rclcpp::Node, NebulaDrive /// @param msg The new ContinentalArs548ObjectList from the driver void ObjectListCallback(std::unique_ptr msg); + /// @brief Callback to process new ContinentalARS548Status from the driver + /// @param msg The new ContinentalArs548ObjectList from the driver + void SensorStatusCallback( + const drivers::continental_ars548::ContinentalARS548Status & sensor_status); + public: explicit ContinentalARS548DriverRosWrapper(const rclcpp::NodeOptions & options); diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp index ed2ed3964..f90a655bf 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -17,8 +17,6 @@ #include #include -#include -#include #include #include #include @@ -71,7 +69,7 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; Status interface_status_; - drivers::ContinentalARS548SensorConfiguration sensor_configuration_; + drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration_; /// @brief Received Continental Radar message publisher rclcpp::Publisher::SharedPtr packets_pub_; @@ -139,7 +137,7 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; /// @brief Stop point cloud streaming (not used) @@ -151,17 +149,13 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, /// @brief Get configurations from ros parameters /// @param sensor_configuration Output of SensorConfiguration /// @return Resulting status - Status GetParameters(drivers::ContinentalARS548SensorConfiguration & sensor_configuration); + Status GetParameters( + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); private: std::mutex mtx_config_; OnSetParametersCallbackHandle::SharedPtr set_param_res_; - diagnostic_updater::Updater diagnostics_updater_; - - /// @brief Callback to populate diagnostic messages - void ContinentalMonitorStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..41e34a587 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -0,0 +1,143 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_ars548::MultiContinentalARS548HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + std::unordered_map::SharedPtr> + packets_pub_map_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + + bool standstill_{true}; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback( + std::unique_ptr packets_buffer, const std::string & sensor_ip); + + /// @brief Callback to send the odometry information to the radar device + /// @param msg The odometry message + void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the acceleration information to the radar device + /// @param msg The acceleration message + void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the steering angle information to the radar device + /// @param msg The steering angle message + void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + +public: + explicit MultiContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~MultiContinentalARS548HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp index 52518fcd0..0a6118df4 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.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_HesaiDriverRosWrapper_H #define NEBULA_HesaiDriverRosWrapper_H @@ -13,11 +27,13 @@ #include #include -#include - #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" +#include +#include +#include + namespace nebula { namespace ros diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index 91caca085..c0598fc69 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -1,11 +1,25 @@ +// 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_HesaiHwInterfaceRosWrapper_H #define NEBULA_HesaiHwInterfaceRosWrapper_H +#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" #include #include @@ -16,8 +30,11 @@ #include +#include #include +#include #include +#include namespace nebula { @@ -65,7 +82,7 @@ class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceW public: explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); ~HesaiHwInterfaceRosWrapper() noexcept override; - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; /// @brief Stop point cloud streaming (not used) diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index 857f638cd..f3ba88b24 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -1,25 +1,40 @@ +// 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_HesaiHwMonitorRosWrapper_H #define NEBULA_HesaiHwMonitorRosWrapper_H +#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" #include #include #include #include -#include - -#include #include #include #include +#include +#include +#include #include +#include namespace nebula { diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp index 2095d75fb..19aa36f54 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp @@ -62,7 +62,7 @@ class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterf public: explicit RobosenseHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp index a3606c091..6576332b5 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.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_VelodyneHwInterfaceRosWrapper_H #define NEBULA_VelodyneHwInterfaceRosWrapper_H @@ -16,7 +30,10 @@ #include #include +#include #include +#include +#include namespace nebula { @@ -64,7 +81,7 @@ class VelodyneHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfa public: explicit VelodyneHwInterfaceRosWrapper(const rclcpp::NodeOptions & options); - /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status Status StreamStart() override; /// @brief Stop point cloud streaming (not used) diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index fb603d137..875ced1b5 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -8,6 +8,7 @@ + @@ -29,59 +30,171 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index cf7aff404..89e6b7f31 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -24,7 +24,7 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( const rclcpp::NodeOptions & options) : rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() { - drivers::ContinentalARS548SensorConfiguration sensor_configuration; + drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -38,10 +38,12 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Starting..."); sensor_cfg_ptr_ = - std::make_shared(sensor_configuration); + std::make_shared( + sensor_configuration); wrapper_status_ = InitializeDriver( - std::const_pointer_cast(sensor_cfg_ptr_)); + std::const_pointer_cast( + sensor_cfg_ptr_)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); packets_sub_ = create_subscription( @@ -68,6 +70,9 @@ ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( objects_markers_pub_ = this->create_publisher("marker_array", 10); + + diagnostics_pub_ = + this->create_publisher("diagnostics", 10); } void ContinentalARS548DriverRosWrapper::ReceivePacketsMsgCallback( @@ -80,12 +85,15 @@ Status ContinentalARS548DriverRosWrapper::InitializeDriver( std::shared_ptr sensor_configuration) { decoder_ptr_ = std::make_shared( - std::static_pointer_cast(sensor_configuration)); + std::static_pointer_cast( + sensor_configuration)); decoder_ptr_->RegisterDetectionListCallback(std::bind( &ContinentalARS548DriverRosWrapper::DetectionListCallback, this, std::placeholders::_1)); decoder_ptr_->RegisterObjectListCallback( std::bind(&ContinentalARS548DriverRosWrapper::ObjectListCallback, this, std::placeholders::_1)); + decoder_ptr_->RegisterSensorStatusCallback(std::bind( + &ContinentalARS548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); return Status::OK; } @@ -96,7 +104,7 @@ Status ContinentalARS548DriverRosWrapper::GetStatus() } Status ContinentalARS548DriverRosWrapper::GetParameters( - drivers::ContinentalARS548SensorConfiguration & sensor_configuration) + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) { { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -277,7 +285,8 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( } std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration); + std::make_shared( + sensor_configuration); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); @@ -351,6 +360,120 @@ void ContinentalARS548DriverRosWrapper::ObjectListCallback( } } +void ContinentalARS548DriverRosWrapper::SensorStatusCallback( + const drivers::continental_ars548::ContinentalARS548Status & sensor_status) +{ + diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; + diagnostic_array_msg.header.stamp.sec = sensor_status.timestamp_seconds; + diagnostic_array_msg.header.stamp.nanosec = sensor_status.timestamp_nanoseconds; + diagnostic_array_msg.header.frame_id = sensor_cfg_ptr_->frame_id; + + diagnostic_array_msg.status.resize(1); + auto & status = diagnostic_array_msg.status[0]; + status.values.reserve(36); + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.hardware_id = sensor_cfg_ptr_->frame_id; + status.name = sensor_cfg_ptr_->frame_id; + status.message = "Diagnostic messages from ARS548"; + + auto add_diagnostic = [&status](const std::string & key, const std::string & value) { + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + status.values.push_back(key_value); + }; + + add_diagnostic("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); + add_diagnostic("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); + add_diagnostic("timestamp_sync_status", sensor_status.timestamp_sync_status); + add_diagnostic("sw_version_major", std::to_string(sensor_status.sw_version_major)); + add_diagnostic("sw_version_minor", std::to_string(sensor_status.sw_version_minor)); + add_diagnostic("sw_version_patch", std::to_string(sensor_status.sw_version_patch)); + add_diagnostic("longitudinal", std::to_string(sensor_status.longitudinal)); + add_diagnostic("lateral", std::to_string(sensor_status.lateral)); + add_diagnostic("vertical", std::to_string(sensor_status.vertical)); + add_diagnostic("yaw", std::to_string(sensor_status.yaw)); + add_diagnostic("pitch", std::to_string(sensor_status.pitch)); + add_diagnostic("plug_orientation", sensor_status.plug_orientation); + add_diagnostic("length", std::to_string(sensor_status.length)); + add_diagnostic("width", std::to_string(sensor_status.width)); + add_diagnostic("height", std::to_string(sensor_status.height)); + add_diagnostic("wheel_base", std::to_string(sensor_status.wheel_base)); + add_diagnostic("max_distance", std::to_string(sensor_status.max_distance)); + add_diagnostic("frequency_slot", sensor_status.frequency_slot); + add_diagnostic("cycle_time", std::to_string(sensor_status.cycle_time)); + add_diagnostic("time_slot", std::to_string(sensor_status.time_slot)); + add_diagnostic("hcc", sensor_status.hcc); + add_diagnostic("power_save_standstill", sensor_status.power_save_standstill); + add_diagnostic("sensor_ip_address0", sensor_status.sensor_ip_address0); + add_diagnostic("sensor_ip_address1", sensor_status.sensor_ip_address1); + add_diagnostic("configuration_counter", std::to_string(sensor_status.configuration_counter)); + add_diagnostic("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); + add_diagnostic( + "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); + add_diagnostic("lateral_acceleration_status", sensor_status.lateral_acceleration_status); + add_diagnostic("yaw_rate_status", sensor_status.yaw_rate_status); + add_diagnostic("steering_angle_status", sensor_status.steering_angle_status); + add_diagnostic("driving_direction_status", sensor_status.driving_direction_status); + add_diagnostic("characteristic_speed_status", sensor_status.characteristic_speed_status); + add_diagnostic("radar_status", sensor_status.radar_status); + add_diagnostic("voltage_status", sensor_status.voltage_status); + add_diagnostic("temperature_status", sensor_status.temperature_status); + add_diagnostic("blockage_status", sensor_status.blockage_status); + + double detection_total_time_sec = + (sensor_status.detection_last_stamp - sensor_status.detection_first_stamp) * 1e-9; + uint64_t expected_total_detection = + static_cast(detection_total_time_sec / (sensor_status.cycle_time * 1e-3)); + uint64_t detection_count_diff = + expected_total_detection > sensor_status.detection_total_count + ? expected_total_detection - sensor_status.detection_total_count + : sensor_status.detection_total_count - expected_total_detection; + double detection_dropped_rate = + 100.0 * std::abs(detection_count_diff) / expected_total_detection; + double detection_dropped_rate_dt = + 100.0 * sensor_status.detection_dropped_dt_count / sensor_status.detection_total_count; + double detection_empty_rate = + 100.0 * sensor_status.detection_empty_count / sensor_status.detection_total_count; + + add_diagnostic("detection_total_time", std::to_string(detection_total_time_sec)); + add_diagnostic("detection_dropped_rate", std::to_string(detection_dropped_rate)); + add_diagnostic("detection_dropped_rate_dt", std::to_string(detection_dropped_rate_dt)); + add_diagnostic("detection_empty_rate", std::to_string(detection_empty_rate)); + add_diagnostic( + "detection_dropped_dt_count", std::to_string(sensor_status.detection_dropped_dt_count)); + add_diagnostic("detection_empty_count", std::to_string(sensor_status.detection_empty_count)); + + double object_total_time_sec = + (sensor_status.object_last_stamp - sensor_status.object_first_stamp) * 1e-9; + uint64_t expected_total_object = + static_cast(object_total_time_sec / (sensor_status.cycle_time * 1e-3)); + uint64_t object_count_diff = expected_total_object > sensor_status.object_total_count + ? expected_total_object - sensor_status.object_total_count + : sensor_status.object_total_count - expected_total_object; + double object_dropped_rate = 100.0 * std::abs(object_count_diff) / expected_total_object; + double object_dropped_rate_dt = + 100.0 * sensor_status.object_dropped_dt_count / sensor_status.object_total_count; + double object_empty_rate = + 100.0 * sensor_status.object_empty_count / sensor_status.object_total_count; + + add_diagnostic("sensor_status.expected_total_object", std::to_string(expected_total_object)); + add_diagnostic( + "sensor_status.detection_total_count", std::to_string(sensor_status.detection_total_count)); + + add_diagnostic("object_total_time", std::to_string(object_total_time_sec)); + add_diagnostic("object_dropped_rate", std::to_string(object_dropped_rate)); + add_diagnostic("object_dropped_rate_dt", std::to_string(object_dropped_rate_dt)); + add_diagnostic("object_empty_rate", std::to_string(object_empty_rate)); + add_diagnostic("object_dropped_dt_count", std::to_string(sensor_status.object_dropped_dt_count)); + add_diagnostic("object_empty_count", std::to_string(sensor_status.object_empty_count)); + + add_diagnostic("status_total_count", std::to_string(sensor_status.status_total_count)); + add_diagnostic("radar_invalid_count", std::to_string(sensor_status.radar_invalid_count)); + + diagnostics_pub_->publish(diagnostic_array_msg); +} + pcl::PointCloud::Ptr ContinentalARS548DriverRosWrapper::ConvertToPointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) @@ -401,19 +524,8 @@ ContinentalARS548DriverRosWrapper::ConvertToPointcloud( nebula::drivers::continental_ars548::PointARS548Object point{}; for (const auto & object : msg.objects) { - const double half_length = 0.5 * object.shape_length_edge_mean; - const double half_width = 0.5 * object.shape_width_edge_mean; - const int reference_index = std::min(object.position_reference, 8); - const double & yaw = object.orientation; - const double x = object.position.x + - std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - - std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; - const double y = object.position.y + - std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + - std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; - - point.x = static_cast(x); - point.y = static_cast(y); + point.x = static_cast(object.position.x); + point.y = static_cast(object.position.y); point.z = static_cast(object.position.z); point.id = object.object_id; @@ -621,7 +733,6 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT radar_msgs::msg::RadarTrack track_msg; for (const auto & object : msg.objects) { - const Eigen::Vector2d center_xy{object.position.x, object.position.y}; const double half_length = 0.5 * object.shape_length_edge_mean; const double half_width = 0.5 * object.shape_width_edge_mean; constexpr double DEFAULT_HALF_SIZE = 1.0; diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index f4a83ba81..6ae01bd3d 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -26,9 +26,7 @@ namespace ros { ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( const rclcpp::NodeOptions & options) -: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), - hw_interface_(), - diagnostics_updater_(this) +: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() { if (mtx_config_.try_lock()) { interface_status_ = GetParameters(sensor_configuration_); @@ -39,8 +37,10 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( return; } hw_interface_.SetLogger(std::make_shared(this->get_logger())); - std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); @@ -63,7 +63,7 @@ ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper( Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.CloudInterfaceStart(); + interface_status_ = hw_interface_.SensorInterfaceStart(); } if (Status::OK == interface_status_) { @@ -106,11 +106,6 @@ Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() std::bind( &ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback, this, std::placeholders::_1, std::placeholders::_2)); - - std::scoped_lock lock(mtx_config_); - diagnostics_updater_.add( - "radar_status(" + sensor_configuration_.frame_id + ")", this, - &ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus); } return interface_status_; @@ -136,7 +131,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: } Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( - drivers::ContinentalARS548SensorConfiguration & sensor_configuration) + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) { { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -354,10 +349,6 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( return Status::INVALID_SENSOR_MODEL; } - if (sensor_configuration.frame_id.empty()) { - return Status::SENSOR_CONFIG_ERROR; - } - RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); return Status::OK; } @@ -377,15 +368,31 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); RCLCPP_INFO_STREAM(this->get_logger(), p); - drivers::ContinentalARS548SensorConfiguration new_param{sensor_configuration_}; + drivers::continental_ars548::ContinentalARS548SensorConfiguration new_param{ + sensor_configuration_}; RCLCPP_INFO_STREAM(this->get_logger(), new_param); std::string sensor_model_str; - std::string return_mode_str; + if ( - get_param(p, "sensor_model", sensor_model_str) | get_param(p, "return_mode", return_mode_str) | - get_param(p, "host_ip", new_param.host_ip) | get_param(p, "sensor_ip", new_param.sensor_ip) | + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | get_param(p, "new_sensor_ip", new_param.new_sensor_ip) | - get_param(p, "frame_id", new_param.frame_id) | get_param(p, "data_port", new_param.data_port) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "new_plug_orientation", new_param.new_plug_orientation) | + get_param(p, "new_vehicle_length", new_param.new_vehicle_length) | + get_param(p, "new_vehicle_width", new_param.new_vehicle_width) | + get_param(p, "new_vehicle_height", new_param.new_vehicle_height) | + get_param(p, "new_vehicle_wheelbase", new_param.new_vehicle_wheelbase) | + get_param(p, "new_radar_maximum_distance", new_param.new_radar_maximum_distance) | + get_param(p, "new_radar_frequency_slot", new_param.new_radar_frequency_slot) | + get_param(p, "new_radar_cycle_time", new_param.new_radar_cycle_time) | + get_param(p, "new_radar_time_slot", new_param.new_radar_time_slot) | + get_param(p, "new_radar_country_code", new_param.new_radar_time_slot) | + get_param(p, "new_radar_powersave_standstill", new_param.new_radar_time_slot) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { if (0 < sensor_model_str.length()) @@ -394,7 +401,8 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: sensor_configuration_ = new_param; RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); std::shared_ptr sensor_cfg_ptr = - std::make_shared(sensor_configuration_); + std::make_shared( + sensor_configuration_); RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); @@ -414,8 +422,6 @@ void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { std::scoped_lock lock(mtx_config_); - // hw_interface_.SetCharacteristicSpeed(0.0); - // hw_interface_.SetSteeringAngleFrontAxle(0.0); constexpr float speed_to_standstill = 0.5f; constexpr float speed_to_moving = 2.f; @@ -429,11 +435,11 @@ void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( if (standstill_) { hw_interface_.SetDrivingDirection(0); } else { - hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x); + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); } constexpr float ms_to_kmh = 3.6f; - hw_interface_.SetVelocityVehicle(ms_to_kmh * msg->twist.twist.linear.x); + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); constexpr float rad_to_deg = 180.f / M_PI; hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); @@ -451,7 +457,8 @@ void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( const std_msgs::msg::Float32::SharedPtr msg) { std::scoped_lock lock(mtx_config_); - hw_interface_.SetSteeringAngleFrontAxle(msg->data); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); } void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback( @@ -527,59 +534,31 @@ ContinentalARS548HwInterfaceRosWrapper::updateParameters() {rclcpp::Parameter("sensor_model", os_sensor_model.str()), rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), - rclcpp::Parameter("new_sensor_ip", sensor_configuration_.new_sensor_ip), rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("new_sensor_ip", sensor_configuration_.new_sensor_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), rclcpp::Parameter( - "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), + rclcpp::Parameter("new_plug_orientation", sensor_configuration_.new_plug_orientation), + rclcpp::Parameter("new_vehicle_length", sensor_configuration_.new_vehicle_length), + rclcpp::Parameter("new_vehicle_width", sensor_configuration_.new_vehicle_width), + rclcpp::Parameter("new_vehicle_height", sensor_configuration_.new_vehicle_height), + rclcpp::Parameter("new_vehicle_wheelbase", sensor_configuration_.new_vehicle_wheelbase), + rclcpp::Parameter( + "new_radar_maximum_distance", sensor_configuration_.new_radar_maximum_distance), + rclcpp::Parameter("new_radar_frequency_slot", sensor_configuration_.new_radar_frequency_slot), + rclcpp::Parameter("new_radar_cycle_time", sensor_configuration_.new_radar_cycle_time), + rclcpp::Parameter("new_radar_time_slot", sensor_configuration_.new_radar_time_slot), + rclcpp::Parameter("new_radar_country_code", sensor_configuration_.new_radar_country_code), + rclcpp::Parameter( + "new_radar_powersave_standstill", sensor_configuration_.new_radar_powersave_standstill)}); RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); return results; } -void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( - diagnostic_updater::DiagnosticStatusWrapper & diagnostics) -{ - auto sensor_status = hw_interface_.GetRadarStatus(); - diagnostics.add("timestamp_nanoseconds", std::to_string(sensor_status.timestamp_nanoseconds)); - diagnostics.add("timestamp_seconds", std::to_string(sensor_status.timestamp_seconds)); - diagnostics.add("timestamp_sync_status", sensor_status.timestamp_sync_status); - diagnostics.add("sw_version_major", std::to_string(sensor_status.sw_version_major)); - diagnostics.add("sw_version_minor", std::to_string(sensor_status.sw_version_minor)); - diagnostics.add("sw_version_patch", std::to_string(sensor_status.sw_version_patch)); - diagnostics.add("longitudinal", std::to_string(sensor_status.longitudinal)); - diagnostics.add("lateral", std::to_string(sensor_status.lateral)); - diagnostics.add("vertical", std::to_string(sensor_status.vertical)); - diagnostics.add("yaw", std::to_string(sensor_status.yaw)); - diagnostics.add("pitch", std::to_string(sensor_status.pitch)); - diagnostics.add("plug_orientation", sensor_status.plug_orientation); - diagnostics.add("length", std::to_string(sensor_status.length)); - diagnostics.add("width", std::to_string(sensor_status.width)); - diagnostics.add("height", std::to_string(sensor_status.height)); - diagnostics.add("wheel_base", std::to_string(sensor_status.wheel_base)); - diagnostics.add("max_distance", std::to_string(sensor_status.max_distance)); - diagnostics.add("frequency_slot", sensor_status.frequency_slot); - diagnostics.add("cycle_time", std::to_string(sensor_status.cycle_time)); - diagnostics.add("time_slot", std::to_string(sensor_status.time_slot)); - diagnostics.add("hcc", sensor_status.hcc); - diagnostics.add("power_save_standstill", sensor_status.power_save_standstill); - diagnostics.add("sensor_ip_address0", sensor_status.sensor_ip_address0); - diagnostics.add("sensor_ip_address1", sensor_status.sensor_ip_address1); - diagnostics.add("configuration_counter", std::to_string(sensor_status.configuration_counter)); - diagnostics.add("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); - diagnostics.add( - "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); - diagnostics.add("lateral_acceleration_status", sensor_status.lateral_acceleration_status); - diagnostics.add("yaw_rate_status", sensor_status.yaw_rate_status); - diagnostics.add("steering_angle_status", sensor_status.steering_angle_status); - diagnostics.add("driving_direction_status", sensor_status.driving_direction_status); - diagnostics.add("characteristic_speed_status", sensor_status.characteristic_speed_status); - diagnostics.add("radar_status", sensor_status.radar_status); - diagnostics.add("voltage_status", sensor_status.voltage_status); - diagnostics.add("temperature_status", sensor_status.temperature_status); - diagnostics.add("blockage_status", sensor_status.blockage_status); -} - RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp new file mode 100644 index 000000000..58db41634 --- /dev/null +++ b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp @@ -0,0 +1,345 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +MultiContinentalARS548HwInterfaceRosWrapper::MultiContinentalARS548HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); + hw_interface_.RegisterScanCallback(std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); + sensor_id++) { + const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; + const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; + packets_pub_map_[sensor_ip] = this->create_publisher( + frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); + } + + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +MultiContinentalARS548HwInterfaceRosWrapper::~MultiContinentalARS548HwInterfaceRosWrapper() +{ +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, + std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + } + + return interface_status_; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status MultiContinentalARS548HwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("sensor_ips", descriptor); + sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("frame_ids", descriptor); + sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer, const std::string & sensor_ip) +{ + packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ips", new_param.sensor_ips) | + get_param(p, "frame_ids", new_param.frame_ids) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +std::vector +MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), + rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalARS548HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index d8e206995..a878a001c 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -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. + #include "nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp" namespace nebula @@ -64,7 +78,7 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; - }; + } if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { @@ -88,8 +102,8 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); + const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = @@ -98,7 +112,8 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( } auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); + RCLCPP_DEBUG( + get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiDriverRosWrapper::PublishCloud( @@ -135,7 +150,10 @@ Status HesaiDriverRosWrapper::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } +Status HesaiDriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} Status HesaiDriverRosWrapper::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -257,48 +275,53 @@ Status HesaiDriverRosWrapper::GetParameters( hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - + bool run_local = false; RCLCPP_INFO_STREAM( - this->get_logger(), "Trying to acquire calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); - std::cout << "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip << "'" << std::endl; + this->get_logger(), + "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip << "'"); + std::cout << "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip + << "'" << std::endl; if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; if (!calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += + calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( + ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &calibration_configuration, &calibration_file_path_from_sensor](const std::string & str) { - auto rt = calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); - } - }, - true); - }else{ - run_local = true; - } - }); + std::future future = std::async( + std::launch::async, + [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + hw_interface_.GetLidarCalibrationFromSensor( + [this, &calibration_configuration, + &calibration_file_path_from_sensor](const std::string & str) { + auto rt = calibration_configuration.SaveFileFromString( + calibration_file_path_from_sensor, str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); + } else { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); + } + }, + true); + } else { + run_local = true; + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); if (status == std::future_status::timeout) { @@ -306,32 +329,32 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired calibration data from sensor: '" - << sensor_configuration.sensor_ip << "'"); + this->get_logger(), + "Acquired calibration data from sensor: '" << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The calibration has been saved in '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); } - if(run_local) { + if (run_local) { bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = - calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_file_path_from_sensor << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); } } - if(run_org) { + if (run_org) { if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = @@ -342,55 +365,65 @@ Status HesaiDriverRosWrapper::GetParameters( this->get_logger(), "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load calibration data from: '" - << calibration_configuration.calibration_file << "'"); + this->get_logger(), + "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); } } } } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 run_local = true; std::string correction_file_path_from_sensor; if (!correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + correction_file_path_from_sensor += + correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &correction_configuration, &correction_file_path_from_sensor, &run_local](const std::vector & received_bytes) { - RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if(rt == Status::OK) - { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); - run_local = false; - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); - run_local = true; - } - }); - }else{ - RCLCPP_ERROR_STREAM(get_logger(), "InitializeTcpDriver failed. Falling back to offline calibration file."); - run_local = true; - } - }); + std::future future = std::async( + std::launch::async, + [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + hw_interface_.GetLidarCalibrationFromSensor( + [this, &correction_configuration, &correction_file_path_from_sensor, + &run_local](const std::vector & received_bytes) { + RCLCPP_INFO_STREAM( + get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary( + correction_file_path_from_sensor, received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), + "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "SaveFileFromBinary failed:" << correction_file_path_from_sensor + << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if (rt == Status::OK) { + RCLCPP_INFO_STREAM( + get_logger(), "LoadFromBinary success" + << "\n"); + run_local = false; + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "LoadFromBinary failed" + << ". Falling back to offline calibration file."); + run_local = true; + } + }); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "InitializeTcpDriver failed. Falling back to offline calibration file."); + run_local = true; + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); if (status == std::future_status::timeout) { @@ -398,29 +431,28 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), "Acquired correction data from sensor: '" - << sensor_configuration.sensor_ip << "'"); + this->get_logger(), + "Acquired correction data from sensor: '" << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), "The correction has been saved in '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "The correction has been saved in '" << correction_file_path_from_sensor << "'"); } - if(run_local) { + if (run_local) { bool run_org = false; if (correction_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = - correction_configuration.LoadFromFile(correction_file_path_from_sensor); + auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path_from_sensor << "'"); + this->get_logger(), + "Load correction data from: '" << correction_file_path_from_sensor << "'"); } } - if(run_org) { + if (run_org) { if (correction_file_path.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); @@ -432,15 +464,14 @@ Status HesaiDriverRosWrapper::GetParameters( RCLCPP_ERROR_STREAM( this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); return cal_status; - }else{ + } else { RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" - << correction_file_path << "'"); + this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); } } } } - } // end AT128 + } // end AT128 // Do not use outside of this location hw_interface_.~HesaiHwInterface(); RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index f3d706c09..6165c2607 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -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. + #include "nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp" namespace nebula @@ -23,36 +37,31 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) Status rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); - if(this->retry_hw_) - { + if (this->retry_hw_) { int cnt = 0; RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while(rt == Status::ERROR_1) - { + while (rt == Status::ERROR_1) { cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); } } - if(rt != Status::ERROR_1){ - try{ + if (rt != Status::ERROR_1) { + try { std::vector thread_pool{}; - thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - - } - catch (...) - { + thread_pool.emplace_back([this] { + hw_interface_.GetInventory( // ios, + [this](HesaiInventory & result) { + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); + }); + }); + for (std::thread & th : thread_pool) { + th.join(); + } + } catch (...) { std::cout << "catch (...) in parent" << std::endl; RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); } @@ -60,10 +69,10 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions hw_interface_.CheckAndSetConfig(); updateParameters(); } - } - else - { - RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), + "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); } #endif @@ -147,7 +156,8 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions StreamStart(); } -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { +HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() +{ RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); hw_interface_.FinalizeTcpDriver(); } @@ -155,13 +165,19 @@ HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { Status HesaiHwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.CloudInterfaceStart(); + interface_status_ = hw_interface_.SensorInterfaceStart(); } return interface_status_; } -Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } +Status HesaiHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status HesaiHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index 82be03022..e723376d7 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -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. + #include "nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp" namespace nebula @@ -61,9 +75,8 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o hw_interface_.SetLogger(std::make_shared(this->get_logger())); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - while(hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) - { - std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 + while (hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) { + std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 } std::vector thread_pool{}; thread_pool.emplace_back([this] { @@ -89,10 +102,19 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); } -Status HesaiHwMonitorRosWrapper::MonitorStart() { return interface_status_; } +Status HesaiHwMonitorRosWrapper::MonitorStart() +{ + return interface_status_; +} -Status HesaiHwMonitorRosWrapper::MonitorStop() { return Status::OK; } -Status HesaiHwMonitorRosWrapper::Shutdown() { return Status::OK; } +Status HesaiHwMonitorRosWrapper::MonitorStop() +{ + return Status::OK; +} +Status HesaiHwMonitorRosWrapper::Shutdown() +{ + return Status::OK; +} Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -613,11 +635,12 @@ void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( } } - HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); - } +HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() +{ + RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); + hw_interface_.FinalizeTcpDriver(); +} - RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) +RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp index c5a2eb06e..d0655d3e5 100644 --- a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp @@ -58,7 +58,7 @@ Status RobosenseHwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { RCLCPP_INFO_STREAM(get_logger(), "Starting interface."); - hw_interface_.CloudInterfaceStart(); + hw_interface_.SensorInterfaceStart(); hw_interface_.InfoInterfaceStart(); } return interface_status_; diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp index d70a5e045..17903764e 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -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. + #include "nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp" namespace nebula @@ -57,13 +71,19 @@ VelodyneHwInterfaceRosWrapper::VelodyneHwInterfaceRosWrapper(const rclcpp::NodeO Status VelodyneHwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.CloudInterfaceStart(); + interface_status_ = hw_interface_.SensorInterfaceStart(); } return interface_status_; } -Status VelodyneHwInterfaceRosWrapper::StreamStop() { return Status::OK; } -Status VelodyneHwInterfaceRosWrapper::Shutdown() { return Status::OK; } +Status VelodyneHwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status VelodyneHwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} Status VelodyneHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -269,7 +289,6 @@ rcl_interfaces::msg::SetParametersResult VelodyneHwInterfaceRosWrapper::paramCal get_param(p, "rotation_speed", new_param.rotation_speed) || get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || get_param(p, "cloud_max_angle", new_param.cloud_max_angle)) { // || - if (0 < sensor_model_str.length()) new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); if (0 < return_mode_str.length()) From 2801c8bf3412fbef0e9c0cd482519d6bd09b1084 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 16 Feb 2024 18:12:34 +0900 Subject: [PATCH 22/42] chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp index 980e2d627..24c693978 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp @@ -56,7 +56,7 @@ class NebulaHwInterfaceWrapperBase virtual Status InitializeHwInterface( const drivers::SensorConfigurationBase & sensor_configuration) = 0; // void SendDataPacket(const std::vector &buffer); // Ideally this will be - // implemented as specific funtions, GetFanStatus, GetEchoMode + // implemented as specific functions, GetFanStatus, GetEchoMode /// @brief Enable sensor setup during initialization and set_parameters_callback bool setup_sensor; From 61cbd3840f8703a6763525760437d470471fa932 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 6 Mar 2024 17:38:16 +0900 Subject: [PATCH 23/42] fix: covariance matrix Signed-off-by: Kenzo Lobos-Tsunekawa --- ...continental_ars548_decoder_ros_wrapper.cpp | 27 ++++++++++++------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 89e6b7f31..f06929576 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -644,26 +644,35 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr track_msg.classification = PEDESTRIAN_ID; } - track_msg.position_covariance[0] = static_cast(object.position_std.x); + track_msg.position_covariance[0] = + static_cast(object.position_std.x * object.position_std.x); track_msg.position_covariance[1] = object.position_covariance_xy; track_msg.position_covariance[2] = 0.f; - track_msg.position_covariance[3] = static_cast(object.position_std.y); + track_msg.position_covariance[3] = + static_cast(object.position_std.y * object.position_std.y); track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = static_cast(object.position_std.z); + track_msg.position_covariance[5] = + static_cast(object.position_std.z * object.position_std.z); - track_msg.velocity_covariance[0] = static_cast(object.absolute_velocity_std.x); + track_msg.velocity_covariance[0] = + static_cast(object.absolute_velocity_std.x * object.absolute_velocity_std.x); track_msg.velocity_covariance[1] = object.absolute_velocity_covariance_xy; track_msg.velocity_covariance[2] = 0.f; - track_msg.velocity_covariance[3] = static_cast(object.absolute_velocity_std.y); + track_msg.velocity_covariance[3] = + static_cast(object.absolute_velocity_std.y * object.absolute_velocity_std.y); track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = static_cast(object.absolute_velocity_std.z); + track_msg.velocity_covariance[5] = + static_cast(object.absolute_velocity_std.z * object.absolute_velocity_std.z); - track_msg.acceleration_covariance[0] = static_cast(object.absolute_acceleration_std.x); + track_msg.acceleration_covariance[0] = + static_cast(object.absolute_acceleration_std.x * object.absolute_acceleration_std.x); track_msg.acceleration_covariance[1] = object.absolute_acceleration_covariance_xy; track_msg.acceleration_covariance[2] = 0.f; - track_msg.acceleration_covariance[3] = static_cast(object.absolute_acceleration_std.y); + track_msg.acceleration_covariance[3] = + static_cast(object.absolute_acceleration_std.y * object.absolute_acceleration_std.y); track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = static_cast(object.absolute_acceleration_std.z); + track_msg.acceleration_covariance[5] = + static_cast(object.absolute_acceleration_std.z * object.absolute_acceleration_std.z); track_msg.size_covariance[0] = INVALID_COVARIANCE; track_msg.size_covariance[1] = 0.f; From ec80eab73f575aad21c9521e0db734a0474ef639 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 6 Mar 2024 17:49:15 +0900 Subject: [PATCH 24/42] chore: added documentation on the ros wrapper regarding the reference point Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/continental/continental_ars548_decoder_ros_wrapper.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index f06929576..6218896f7 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -604,6 +604,8 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr const double half_length = 0.5 * object.shape_length_edge_mean; const double half_width = 0.5 * object.shape_width_edge_mean; + // There are 9 possible reference points. In the case of an invalid refence point, we fall back + // to the center const int reference_index = std::min(object.position_reference, 8); const double & yaw = object.orientation; track_msg.position.x = object.position.x + From da81bd619f2f346d53cd6daedc885fc4929ab4f3 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 6 Mar 2024 17:53:00 +0900 Subject: [PATCH 25/42] chore: added documentation regarding the dropped packages estimation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.cpp | 2 ++ 1 file changed, 2 insertions(+) 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 1b3807822..bc56872a0 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 @@ -145,6 +145,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( const uint32_t number_of_detections = detection_list.number_of_detections.value(); msg.detections.resize(number_of_detections); + // Estimate dropped detections only when the radar is synchronzied if (radar_status_.timestamp_sync_status == "SYNC_OK") { if (radar_status_.detection_first_stamp == 0) { radar_status_.detection_first_stamp = @@ -253,6 +254,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( msg.objects.resize(number_of_objects); + // Estimate dropped objects only when the radar is synchronzied if (radar_status_.timestamp_sync_status == "SYNC_OK") { if (radar_status_.object_first_stamp == 0) { radar_status_.object_first_stamp = From 800b351ad5c941b14e31507c4cd59eb6b42ddd4a Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 6 Mar 2024 19:24:15 +0900 Subject: [PATCH 26/42] fix: replaced if-elses by switches and fixed a status vatiable decoding Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 34 +++ .../decoders/continental_ars548_decoder.cpp | 249 +++++++++++------- 2 files changed, 182 insertions(+), 101 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index e4e3f5178..ce1623d47 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -296,6 +296,40 @@ constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; constexpr int MAX_DETECTIONS = 800; constexpr int MAX_OBJECTS = 50; +constexpr int SYNC_OK = 1; +constexpr int NEVER_SYNC = 2; +constexpr int SYNC_LOST = 3; + +constexpr int PLUG_RIGHT = 0; +constexpr int PLUG_LEFT = 1; + +constexpr int FREQUENCY_SLOT_LOW = 0; +constexpr int FREQUENCY_SLOT_MID = 1; +constexpr int FREQUENCY_SLOT_HIGH = 2; + +constexpr int HCC_WORLDWIDE = 1; +constexpr int HCC_JAPAN = 2; + +constexpr int POWERSAVE_STANDSTILL_OFF = 0; +constexpr int POWERSAVE_STANDSTILL_ON = 1; + +constexpr int VDY_OK = 0; +constexpr int VDY_NOTOK = 1; + +constexpr int STATE_INIT = 0; +constexpr int STATE_OK = 1; +constexpr int STATE_INVALID = 2; + +constexpr int BLOCKAGE_STATUS_BLIND = 0; +constexpr int BLOCKAGE_STATUS_HIGH = 1; +constexpr int BLOCKAGE_STATUS_MID = 2; +constexpr int BLOCKAGE_STATUS_LOW = 3; +constexpr int BLOCKAGE_STATUS_NONE = 4; + +constexpr int BLOCKAGE_TEST_FAILED = 0; +constexpr int BLOCKAGE_TEST_PASSED = 1; +constexpr int BLOCKAGE_TEST_ONGOING = 2; + #pragma pack(push, 1) struct HeaderPacket 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 bc56872a0..73f01d247 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 @@ -380,7 +380,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( } bool ContinentalARS548Decoder::ParseSensorStatusPacket( - const std::vector & data, const std_msgs::msg::Header & header) + const std::vector & data, [[maybe_unused]] const std_msgs::msg::Header & header) { SensorStatusPacket sensor_status_packet; std::memcpy(&sensor_status_packet, data.data(), sizeof(SensorStatusPacket)); @@ -388,24 +388,38 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); - if (sensor_status_packet.stamp.timestamp_sync_status == 1) { - radar_status_.timestamp_sync_status = "SYNC_OK"; - } else if (sensor_status_packet.stamp.timestamp_sync_status == 2) { - radar_status_.timestamp_sync_status = "NEVER_SYNC"; - } else if (sensor_status_packet.stamp.timestamp_sync_status == 3) { - radar_status_.timestamp_sync_status = "SYNC_LOST"; - } else { - radar_status_.timestamp_sync_status = "INVALID_VALUE"; + switch (sensor_status_packet.stamp.timestamp_sync_status) { + case SYNC_OK: + radar_status_.timestamp_sync_status = "1:SYNC_OK"; + break; + case NEVER_SYNC: + radar_status_.timestamp_sync_status = "2:NEVER_SYNC"; + break; + case SYNC_LOST: + radar_status_.timestamp_sync_status = "3:SYNC_LOST"; + break; + default: + radar_status_.timestamp_sync_status = + std::to_string(sensor_status_packet.stamp.timestamp_sync_status) + ":Invalid"; + break; } radar_status_.sw_version_major = sensor_status_packet.sw_version_major; radar_status_.sw_version_minor = sensor_status_packet.sw_version_minor; radar_status_.sw_version_patch = sensor_status_packet.sw_version_patch; - radar_status_.plug_orientation = sensor_status_packet.status.plug_orientation == 0 ? "PLUG_RIGHT" - : sensor_status_packet.status.plug_orientation == 1 - ? "PLUG_LEFT" - : "INVALID_VALUE"; + switch (sensor_status_packet.status.plug_orientation) { + case PLUG_RIGHT: + radar_status_.plug_orientation = "0:PLUG_RIGHT"; + break; + case PLUG_LEFT: + radar_status_.plug_orientation = "1:PLUG_LEFT"; + break; + default: + radar_status_.plug_orientation = + std::to_string(sensor_status_packet.status.plug_orientation) + ":Invalid"; + break; + } radar_status_.max_distance = sensor_status_packet.status.maximum_distance.value(); @@ -415,35 +429,54 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.yaw = sensor_status_packet.status.yaw.value(); radar_status_.pitch = sensor_status_packet.status.pitch.value(); - ; radar_status_.length = sensor_status_packet.status.length.value(); - ; radar_status_.width = sensor_status_packet.status.width.value(); radar_status_.height = sensor_status_packet.status.height.value(); radar_status_.wheel_base = sensor_status_packet.status.wheelbase.value(); - if (sensor_status_packet.status.frequency_slot == 0) { - radar_status_.frequency_slot = "0:Low (76.23 GHz)"; - } else if (sensor_status_packet.status.frequency_slot == 1) { - radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; - } else if (sensor_status_packet.status.frequency_slot == 2) { - radar_status_.frequency_slot = "2:High (76.73 GHz)"; - } else { - radar_status_.frequency_slot = "INVALID VALUE"; + switch (sensor_status_packet.status.frequency_slot) { + case FREQUENCY_SLOT_LOW: + radar_status_.frequency_slot = "0:Low (76.23 GHz)"; + break; + case FREQUENCY_SLOT_MID: + radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; + break; + case FREQUENCY_SLOT_HIGH: + radar_status_.frequency_slot = "2:High (76.73 GHz)"; + break; + default: + radar_status_.frequency_slot = + std::to_string(sensor_status_packet.status.frequency_slot) + ":Invalid"; + break; } radar_status_.cycle_time = sensor_status_packet.status.cycle_time; radar_status_.time_slot = sensor_status_packet.status.time_slot; - radar_status_.hcc = sensor_status_packet.status.hcc == 1 ? "Worldwide" - : sensor_status_packet.status.hcc == 2 - ? "Japan" - : ("INVALID VALUE=" + std::to_string(sensor_status_packet.status.hcc)); + switch (sensor_status_packet.status.hcc) { + case HCC_WORLDWIDE: + radar_status_.hcc = "1:Worldwide"; + break; + case HCC_JAPAN: + radar_status_.hcc = "1:Japan"; + break; + default: + radar_status_.hcc = std::to_string(sensor_status_packet.status.hcc) + ":Invalid hcc"; + break; + } - radar_status_.power_save_standstill = - sensor_status_packet.status.powersave_standstill == 0 ? "Off" - : sensor_status_packet.status.powersave_standstill == 1 ? "On" - : "INVALID VALUE"; + switch (sensor_status_packet.status.powersave_standstill) { + case POWERSAVE_STANDSTILL_OFF: + radar_status_.power_save_standstill = "0:Off"; + break; + case POWERSAVE_STANDSTILL_ON: + radar_status_.power_save_standstill = "1:On"; + break; + default: + radar_status_.power_save_standstill = + std::to_string(sensor_status_packet.status.powersave_standstill) + ":Invalid"; + break; + } std::stringstream ss0, ss1; ss0 << std::to_string(sensor_status_packet.status.sensor_ip_address00) << "." @@ -460,107 +493,121 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.configuration_counter = sensor_status_packet.configuration_counter; - radar_status_.longitudinal_velocity_status = - sensor_status_packet.longitudinal_velocity_status == 0 ? "VDY_OK" - : sensor_status_packet.longitudinal_velocity_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; + auto vdy_value_to_string = [](uint8_t value) -> std::string { + switch (value) { + case VDY_OK: + return "0:VDY_OK"; + case VDY_NOTOK: + return "1:VDY_NOTOK"; + default: + return std::to_string(value) + ":Invalid"; + } + }; + radar_status_.longitudinal_velocity_status = + vdy_value_to_string(sensor_status_packet.longitudinal_velocity_status); radar_status_.longitudinal_acceleration_status = - sensor_status_packet.longitudinal_acceleration_status == 0 ? "VDY_OK" - : sensor_status_packet.longitudinal_acceleration_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - + vdy_value_to_string(sensor_status_packet.longitudinal_acceleration_status); radar_status_.lateral_acceleration_status = - sensor_status_packet.lateral_acceleration_status == 0 ? "VDY_OK" - : sensor_status_packet.lateral_acceleration_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.yaw_rate_status = sensor_status_packet.yaw_rate_status == 0 ? "VDY_OK" - : sensor_status_packet.yaw_rate_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - radar_status_.steering_angle_status = sensor_status_packet.steering_angle_status == 0 ? "VDY_OK" - : sensor_status_packet.steering_angle_status == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; + vdy_value_to_string(sensor_status_packet.lateral_acceleration_status); + radar_status_.yaw_rate_status = vdy_value_to_string(sensor_status_packet.yaw_rate_status); + radar_status_.steering_angle_status = + vdy_value_to_string(sensor_status_packet.steering_angle_status); radar_status_.driving_direction_status = - sensor_status_packet.driving_direction_status == 0 ? "VDY_OK" - : sensor_status_packet.driving_direction_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - + vdy_value_to_string(sensor_status_packet.driving_direction_status); radar_status_.characteristic_speed_status = - sensor_status_packet.characteristic_speed_status == 0 ? "VDY_OK" - : sensor_status_packet.characteristic_speed_status == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - if (sensor_status_packet.radar_status == 0) { - radar_status_.radar_status = "STATE_INIT"; - } else if (sensor_status_packet.radar_status == 1) { - radar_status_.radar_status = "STATE_OK"; - } else if (sensor_status_packet.radar_status == 2) { - radar_status_.radar_status = "STATE_INVALID"; - } else { - radar_status_.radar_status = "INVALID VALUE"; + vdy_value_to_string(sensor_status_packet.characteristic_speed_status); + + switch (sensor_status_packet.radar_status) { + case STATE_INIT: + radar_status_.radar_status = "0:STATE_INIT"; + break; + case STATE_OK: + radar_status_.radar_status = "1:STATE_OK"; + break; + case STATE_INVALID: + radar_status_.radar_status = "2:STATE_INVALID"; + break; + default: + radar_status_.radar_status = std::to_string(sensor_status_packet.radar_status) + ":Invalid"; + break; } + std::vector voltage_status_vector, temperature_status_vector; + if (sensor_status_packet.voltage_status == 0) { - radar_status_.voltage_status = "Ok"; + voltage_status_vector.push_back("Ok"); } if (sensor_status_packet.voltage_status & 0x01) { - radar_status_.voltage_status += "Current under voltage"; + voltage_status_vector.push_back("Current undervoltage"); } if (sensor_status_packet.voltage_status & 0x02) { - radar_status_.voltage_status = "Past under voltage"; - } - if (sensor_status_packet.voltage_status & 0x03) { - radar_status_.voltage_status = "Current over voltage"; + voltage_status_vector.push_back("Past undervoltage"); } if (sensor_status_packet.voltage_status & 0x04) { - radar_status_.voltage_status = "Past over voltage"; + voltage_status_vector.push_back("Current overvoltage"); + } + if (sensor_status_packet.voltage_status & 0x08) { + voltage_status_vector.push_back("Past overvoltage"); } if (sensor_status_packet.temperature_status == 0) { - radar_status_.temperature_status = "Ok"; + temperature_status_vector.push_back("Ok"); } if (sensor_status_packet.temperature_status & 0x01) { - radar_status_.temperature_status += "Current under temperature"; + temperature_status_vector.push_back("Current under temperature"); } if (sensor_status_packet.temperature_status & 0x02) { - radar_status_.temperature_status += "Past under temperature"; - } - if (sensor_status_packet.temperature_status & 0x03) { - radar_status_.temperature_status += "Current over temperature"; + temperature_status_vector.push_back("Past under temperature"); } if (sensor_status_packet.temperature_status & 0x04) { - radar_status_.temperature_status += "Past over temperature"; + temperature_status_vector.push_back("Current over temperature"); + } + if (sensor_status_packet.temperature_status & 0x08) { + temperature_status_vector.push_back("Past over temperature"); } + radar_status_.voltage_status = boost::algorithm::join(voltage_status_vector, ", "); + radar_status_.temperature_status = boost::algorithm::join(temperature_status_vector, ", "); + const uint8_t & blockage_status0 = sensor_status_packet.blockage_status & 0x0f; const uint8_t & blockage_status1 = (sensor_status_packet.blockage_status & 0xf0) >> 4; - if (blockage_status0 == 0) { - radar_status_.blockage_status = "Blind"; - } else if (blockage_status0 == 1) { - radar_status_.blockage_status = "High"; - } else if (blockage_status0 == 2) { - radar_status_.blockage_status = "Mid"; - } else if (blockage_status0 == 3) { - radar_status_.blockage_status = "Low"; - } else if (blockage_status0 == 4) { - radar_status_.blockage_status = "None"; - } else { - radar_status_.blockage_status = "INVALID VALUE"; + switch (blockage_status0) { + case BLOCKAGE_STATUS_BLIND: + radar_status_.blockage_status = "0:Blind"; + break; + case BLOCKAGE_STATUS_HIGH: + radar_status_.blockage_status = "1:High"; + break; + case BLOCKAGE_STATUS_MID: + radar_status_.blockage_status = "2:Mid"; + break; + case BLOCKAGE_STATUS_LOW: + radar_status_.blockage_status = "3:Low"; + break; + case BLOCKAGE_STATUS_NONE: + radar_status_.blockage_status = "4:None"; + break; + default: + radar_status_.blockage_status = std::to_string(blockage_status0) + ":Invalid"; + break; } - if (blockage_status1 == 0) { - radar_status_.blockage_status += ". Self test failed"; - } else if (blockage_status1 == 1) { - radar_status_.blockage_status += ". Self test passed"; - } else if (blockage_status1 == 2) { - radar_status_.blockage_status += ". Self test ongoing"; - } else { - radar_status_.blockage_status += ". Invalid self test"; + switch (blockage_status1) { + case BLOCKAGE_TEST_FAILED: + radar_status_.blockage_status += ". 0:Self test failed"; + break; + case BLOCKAGE_TEST_PASSED: + radar_status_.blockage_status += ". 1:Self test passed"; + break; + case BLOCKAGE_TEST_ONGOING: + radar_status_.blockage_status += ". 2:Self test ongoing"; + break; + default: + radar_status_.blockage_status += std::to_string(blockage_status1) + ":Invalid"; + break; } radar_status_.status_total_count++; From 1916f2a95c7bea6c97a40bcc09c969815d7c3b7c Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 6 Mar 2024 19:45:35 +0900 Subject: [PATCH 27/42] chore: spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.cpp | 4 ++-- .../continental/continental_ars548_decoder_ros_wrapper.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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 73f01d247..d1a8cff34 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 @@ -145,7 +145,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( const uint32_t number_of_detections = detection_list.number_of_detections.value(); msg.detections.resize(number_of_detections); - // Estimate dropped detections only when the radar is synchronzied + // Estimate dropped detections only when the radar is synchronized if (radar_status_.timestamp_sync_status == "SYNC_OK") { if (radar_status_.detection_first_stamp == 0) { radar_status_.detection_first_stamp = @@ -254,7 +254,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( msg.objects.resize(number_of_objects); - // Estimate dropped objects only when the radar is synchronzied + // Estimate dropped objects only when the radar is synchronized if (radar_status_.timestamp_sync_status == "SYNC_OK") { if (radar_status_.object_first_stamp == 0) { radar_status_.object_first_stamp = diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 6218896f7..7bafde451 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -604,8 +604,8 @@ radar_msgs::msg::RadarTracks ContinentalARS548DriverRosWrapper::ConvertToRadarTr const double half_length = 0.5 * object.shape_length_edge_mean; const double half_width = 0.5 * object.shape_width_edge_mean; - // There are 9 possible reference points. In the case of an invalid refence point, we fall back - // to the center + // There are 9 possible reference points. In the case of an invalid reference point, we fall + // back to the center const int reference_index = std::min(object.position_reference, 8); const double & yaw = object.orientation; track_msg.position.x = object.position.x + From c7937d596ae976213484c738f86f590d4c0331a2 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 7 Mar 2024 16:08:52 +0900 Subject: [PATCH 28/42] feat: refactored parameter setting into custom services (as much as possible) Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 32 +-- .../continental_ars548_hw_interface.cpp | 46 +++- .../continental_srvs/CMakeLists.txt | 26 ++ nebula_messages/continental_srvs/package.xml | 26 ++ ...ntinentalArs548SetNetworkConfiguration.srv | 4 + .../ContinentalArs548SetRadarParameters.srv | 9 + .../ContinentalArs548SetSensorMounting.srv | 4 + .../ContinentalArs548SetVehicleParameters.srv | 3 + ...nental_ars548_hw_interface_ros_wrapper.hpp | 65 +++-- .../launch/continental_launch_all_hw.xml | 60 ++--- nebula_ros/package.xml | 1 + ...continental_ars548_decoder_ros_wrapper.cpp | 93 +------ ...nental_ars548_hw_interface_ros_wrapper.cpp | 255 +++++++----------- 13 files changed, 289 insertions(+), 335 deletions(-) create mode 100644 nebula_messages/continental_srvs/CMakeLists.txt create mode 100644 nebula_messages/continental_srvs/package.xml create mode 100644 nebula_messages/continental_srvs/srv/ContinentalArs548SetNetworkConfiguration.srv create mode 100644 nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv create mode 100644 nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv create mode 100644 nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index ce1623d47..5dd3b1a11 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -46,22 +46,14 @@ namespace continental_ars548 struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; - std::string new_sensor_ip{}; std::string base_frame{}; uint16_t configuration_host_port{}; uint16_t configuration_sensor_port{}; bool use_sensor_time{}; - uint16_t new_plug_orientation{}; - float new_vehicle_length{}; - float new_vehicle_width{}; - float new_vehicle_height{}; - float new_vehicle_wheelbase{}; - uint16_t new_radar_maximum_distance{}; - uint16_t new_radar_frequency_slot{}; - uint16_t new_radar_cycle_time{}; - uint16_t new_radar_time_slot{}; - uint16_t new_radar_country_code{}; - uint16_t new_radar_powersave_standstill{}; + float configuration_vehicle_length{}; + float configuration_vehicle_width{}; + float configuration_vehicle_height{}; + float configuration_vehicle_wheelbase{}; }; /// @brief struct for Multiple ARS548 sensor configuration @@ -80,21 +72,13 @@ inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip - << ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame << ", ConfigurationHostPort: " << arg.configuration_host_port << ", ConfigurationSensorPort: " << arg.configuration_sensor_port << ", UseSensorTime: " << arg.use_sensor_time - << ", NewPlugOrientation: " << arg.new_plug_orientation - << ", NewVehicleLength: " << arg.new_vehicle_length - << ", NewVehicleWidth: " << arg.new_vehicle_width - << ", NewVehicleHeight: " << arg.new_vehicle_height - << ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase - << ", NewRadarMaximumDistance: " << arg.new_radar_maximum_distance - << ", NewRadarFrequencySlot: " << arg.new_radar_frequency_slot - << ", NewRadarCycleTime: " << arg.new_radar_cycle_time - << ", NewRadarTimeSlot: " << arg.new_radar_time_slot - << ", NewRadarCountryCode: " << arg.new_radar_country_code - << ", RadarPowersaveStandstill: " << arg.new_radar_powersave_standstill; + << ", ConfigurationVehicleLength: " << arg.configuration_vehicle_length + << ", ConfigurationVehicleWidth: " << arg.configuration_vehicle_width + << ", ConfigurationVehicleHeight: " << arg.configuration_vehicle_height + << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; return os; } 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 82ca7a28a..2deeb7a28 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 @@ -220,6 +220,10 @@ Status ContinentalARS548HwInterface::SetSensorMounting( PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -255,6 +259,10 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( PrintInfo("height_autosar = " + std::to_string(height_autosar)); PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -295,6 +303,10 @@ Status ContinentalARS548HwInterface::SetRadarParameters( PrintInfo("hcc = " + std::to_string(hcc)); PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -308,7 +320,7 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens auto sensor_ip = boost::asio::ip::address::from_string(sensor_ip_address); ip_bytes = sensor_ip.to_v4().to_bytes(); } catch (const std::exception & ex) { - PrintError("Setting invalid IP"); + PrintError("Setting invalid IP=" + sensor_ip_address); return Status::SENSOR_CONFIG_ERROR; } @@ -332,6 +344,10 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -359,6 +375,10 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -388,6 +408,10 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit std::memcpy( send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -415,6 +439,10 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic std::vector send_vector(sizeof(CharacteristicSpeedPacket)); std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharacteristicSpeedPacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -447,6 +475,10 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) std::vector send_vector(sizeof(DrivingDirectionPacket)); std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -475,6 +507,10 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) std::memcpy( send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -502,6 +538,10 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) std::vector send_vector(sizeof(VelocityVehiclePacket)); std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; @@ -529,6 +569,10 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) std::vector send_vector(sizeof(YawRatePacket)); std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + sensor_udp_driver_->sender()->asyncSend(send_vector); return Status::OK; diff --git a/nebula_messages/continental_srvs/CMakeLists.txt b/nebula_messages/continental_srvs/CMakeLists.txt new file mode 100644 index 000000000..ffa839825 --- /dev/null +++ b/nebula_messages/continental_srvs/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.5) +project(continental_srvs) + +if (NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif () + +if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif () + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/ContinentalArs548SetSensorMounting.srv" + "srv/ContinentalArs548SetVehicleParameters.srv" + "srv/ContinentalArs548SetRadarParameters.srv" + "srv/ContinentalArs548SetNetworkConfiguration.srv" + DEPENDENCIES + std_msgs + ) + +ament_package() diff --git a/nebula_messages/continental_srvs/package.xml b/nebula_messages/continental_srvs/package.xml new file mode 100644 index 000000000..71ecabc94 --- /dev/null +++ b/nebula_messages/continental_srvs/package.xml @@ -0,0 +1,26 @@ + + + + continental_srvs + 0.1.0 + Services for Continental sensors + Kenzo Lobos-Tsunekawa + + Apache 2 + Tier IV + + ament_cmake + rosidl_default_generators + + builtin_interfaces + geometry_msgs + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetNetworkConfiguration.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetNetworkConfiguration.srv new file mode 100644 index 000000000..d681946fa --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetNetworkConfiguration.srv @@ -0,0 +1,4 @@ +std_msgs/String sensor_ip # must be in the 10.13.1.X network +--- +bool success +string message # status messages diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv new file mode 100644 index 000000000..50ddddc7e --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetRadarParameters.srv @@ -0,0 +1,9 @@ +uint16 maximum_distance +uint16 frequency_slot +uint16 cycle_time +uint16 time_slot +uint16 country_code +uint16 powersave_standstill +--- +bool success +string message # status messages diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv new file mode 100644 index 000000000..8b85557ea --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv @@ -0,0 +1,4 @@ +bool plug_orientation # 0 = PLUG_RIGHT. 1 = PLUG_LEFT +--- +bool success +string message # status messages diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv new file mode 100644 index 000000000..d31c8da81 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv @@ -0,0 +1,3 @@ +--- +bool success +string message # status messages diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp index f90a655bf..f9b66e951 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -23,13 +23,16 @@ #include #include +#include +#include +#include +#include #include #include #include #include #include #include -#include #include @@ -79,10 +82,14 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, bool standstill_{true}; - rclcpp::Service::SharedPtr set_new_sensor_ip_service_server_; - rclcpp::Service::SharedPtr set_new_sensor_mounting_service_server_; - rclcpp::Service::SharedPtr set_new_vehicle_parameters_service_server_; - rclcpp::Service::SharedPtr set_new_radar_parameters_service_server_; + rclcpp::Service::SharedPtr + set_network_configuration_service_server_; + rclcpp::Service::SharedPtr + set_sensor_mounting_service_server_; + rclcpp::Service::SharedPtr + set_vehicle_parameters_service_server_; + rclcpp::Service::SharedPtr + set_radar_parameters_service_server_; /// @brief Initializing hardware interface ros wrapper /// @param sensor_configuration SensorConfiguration for this driver @@ -106,32 +113,40 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); /// @brief Service callback to set the new sensor ip - /// @param request Empty service request - /// @param response Empty service response - void SetNewSensorIPRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + /// @param request service request + /// @param response service response + void SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); /// @brief Service callback to set the new sensor mounting position - /// @param request Empty service request - /// @param response Empty service response - void SetNewSensorMountingRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + /// @param request service request + /// @param response service response + void SetSensorMountingRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); /// @brief Service callback to set the new vehicle parameters - /// @param request Empty service request - /// @param response Empty service response - void SetNewVehicleParametersRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + /// @param request service request + /// @param response service response + void SetVehicleParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); /// @brief Service callback to set the new radar parameters - /// @param request Empty service request - /// @param response Empty service response - void SetNewRadarParametersRequestCallback( - const std::shared_ptr request, - const std::shared_ptr response); + /// @param request service request + /// @param response service response + void SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response); public: explicit ContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 875ced1b5..a8660007a 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -18,17 +18,10 @@ - - - - - - - - - - - + + + + @@ -54,17 +47,10 @@ - - - - - - - - - - - + + + + - - - - - - - - - - - + + + + @@ -113,17 +92,10 @@ - - - - - - - - - - - + + + + ros_environment continental_msgs + continental_srvs diagnostic_msgs diagnostic_updater geometry_msgs diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 7bafde451..04b20f1fa 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -170,25 +170,15 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( this->declare_parameter("use_sensor_time", descriptor); sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_plug_orientation", descriptor); - sensor_configuration.new_plug_orientation = - this->get_parameter("new_plug_orientation").as_int(); - } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_length", descriptor); - sensor_configuration.new_vehicle_length = - static_cast(this->get_parameter("new_vehicle_length").as_double()); + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -196,9 +186,9 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_width", descriptor); - sensor_configuration.new_vehicle_width = - static_cast(this->get_parameter("new_vehicle_width").as_double()); + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -206,9 +196,9 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_height", descriptor); - sensor_configuration.new_vehicle_height = - static_cast(this->get_parameter("new_vehicle_height").as_double()); + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -216,68 +206,9 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_wheelbase", descriptor); - sensor_configuration.new_vehicle_wheelbase = - static_cast(this->get_parameter("new_vehicle_wheelbase").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_maximum_distance", descriptor); - sensor_configuration.new_radar_maximum_distance = - this->get_parameter("new_radar_maximum_distance").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_frequency_slot", descriptor); - sensor_configuration.new_radar_frequency_slot = - this->get_parameter("new_radar_frequency_slot").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_cycle_time", descriptor); - sensor_configuration.new_radar_cycle_time = - this->get_parameter("new_radar_cycle_time").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_time_slot", descriptor); - sensor_configuration.new_radar_time_slot = this->get_parameter("new_radar_time_slot").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_country_code", descriptor); - sensor_configuration.new_radar_country_code = - this->get_parameter("new_radar_country_code").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_powersave_standstill", descriptor); - sensor_configuration.new_radar_powersave_standstill = - this->get_parameter("new_radar_powersave_standstill").as_int(); + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 6ae01bd3d..164cc7bfb 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -84,28 +84,33 @@ Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1)); - set_new_sensor_ip_service_server_ = this->create_service( - "set_new_sensor_ip", std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback, - this, std::placeholders::_1, std::placeholders::_2)); - - set_new_sensor_mounting_service_server_ = this->create_service( - "set_new_sensor_mounting", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_new_vehicle_parameters_service_server_ = this->create_service( - "set_new_vehicle_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); - - set_new_radar_parameters_service_server_ = this->create_service( - "set_new_radar_parameters", - std::bind( - &ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback, this, - std::placeholders::_1, std::placeholders::_2)); + set_network_configuration_service_server_ = + this->create_service( + "set_network_configuration", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + this->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + this->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + this->create_service( + "set_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); } return interface_status_; @@ -161,15 +166,6 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("sensor_ip", descriptor); sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_sensor_ip", descriptor); - sensor_configuration.new_sensor_ip = this->get_parameter("new_sensor_ip").as_string(); - } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; @@ -235,25 +231,15 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("use_sensor_time", descriptor); sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_plug_orientation", descriptor); - sensor_configuration.new_plug_orientation = - this->get_parameter("new_plug_orientation").as_int(); - } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_length", descriptor); - sensor_configuration.new_vehicle_length = - static_cast(this->get_parameter("new_vehicle_length").as_double()); + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -261,9 +247,9 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_width", descriptor); - sensor_configuration.new_vehicle_width = - static_cast(this->get_parameter("new_vehicle_width").as_double()); + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -271,9 +257,9 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_height", descriptor); - sensor_configuration.new_vehicle_height = - static_cast(this->get_parameter("new_vehicle_height").as_double()); + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -281,68 +267,9 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( descriptor.read_only = true; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("new_vehicle_wheelbase", descriptor); - sensor_configuration.new_vehicle_wheelbase = - static_cast(this->get_parameter("new_vehicle_wheelbase").as_double()); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_maximum_distance", descriptor); - sensor_configuration.new_radar_maximum_distance = - this->get_parameter("new_radar_maximum_distance").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_frequency_slot", descriptor); - sensor_configuration.new_radar_frequency_slot = - this->get_parameter("new_radar_frequency_slot").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_cycle_time", descriptor); - sensor_configuration.new_radar_cycle_time = - this->get_parameter("new_radar_cycle_time").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_time_slot", descriptor); - sensor_configuration.new_radar_time_slot = this->get_parameter("new_radar_time_slot").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_country_code", descriptor); - sensor_configuration.new_radar_country_code = - this->get_parameter("new_radar_country_code").as_int(); - } - { - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; - descriptor.read_only = true; - descriptor.dynamic_typing = false; - descriptor.additional_constraints = ""; - this->declare_parameter("new_radar_powersave_standstill", descriptor); - sensor_configuration.new_radar_powersave_standstill = - this->get_parameter("new_radar_powersave_standstill").as_int(); + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -378,21 +305,13 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | get_param(p, "data_port", new_param.data_port) | get_param(p, "multicast_ip", new_param.multicast_ip) | - get_param(p, "new_sensor_ip", new_param.new_sensor_ip) | get_param(p, "base_frame", new_param.base_frame) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | - get_param(p, "new_plug_orientation", new_param.new_plug_orientation) | - get_param(p, "new_vehicle_length", new_param.new_vehicle_length) | - get_param(p, "new_vehicle_width", new_param.new_vehicle_width) | - get_param(p, "new_vehicle_height", new_param.new_vehicle_height) | - get_param(p, "new_vehicle_wheelbase", new_param.new_vehicle_wheelbase) | - get_param(p, "new_radar_maximum_distance", new_param.new_radar_maximum_distance) | - get_param(p, "new_radar_frequency_slot", new_param.new_radar_frequency_slot) | - get_param(p, "new_radar_cycle_time", new_param.new_radar_cycle_time) | - get_param(p, "new_radar_time_slot", new_param.new_radar_time_slot) | - get_param(p, "new_radar_country_code", new_param.new_radar_time_slot) | - get_param(p, "new_radar_powersave_standstill", new_param.new_radar_time_slot) | + get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { if (0 < sensor_model_str.length()) @@ -461,17 +380,22 @@ void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); } -void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback( - [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr response) +void ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) { std::scoped_lock lock(mtx_config_); - hw_interface_.SetSensorIPAddress(sensor_configuration_.new_sensor_ip); + auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback( - [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr response) +void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) { std::scoped_lock lock(mtx_config_); @@ -487,6 +411,8 @@ void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback RCLCPP_ERROR( this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", sensor_configuration_.frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); return; } @@ -494,32 +420,47 @@ void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorMountingRequestCallback geometry_msgs::msg::Vector3 rpy; tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); - hw_interface_.SetSensorMounting( - base_to_sensor_tf.transform.translation.x - sensor_configuration_.new_vehicle_wheelbase, + auto result = hw_interface_.SetSensorMounting( + base_to_sensor_tf.transform.translation.x - + sensor_configuration_.configuration_vehicle_wheelbase, base_to_sensor_tf.transform.translation.y, base_to_sensor_tf.transform.translation.z, rpy.z, - rpy.y, sensor_configuration_.new_plug_orientation); + rpy.y, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceRosWrapper::SetNewVehicleParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr response) +void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) { std::scoped_lock lock(mtx_config_); - hw_interface_.SetVehicleParameters( - sensor_configuration_.new_vehicle_length, sensor_configuration_.new_vehicle_width, - sensor_configuration_.new_vehicle_height, sensor_configuration_.new_vehicle_wheelbase); + auto result = hw_interface_.SetVehicleParameters( + sensor_configuration_.configuration_vehicle_length, + sensor_configuration_.configuration_vehicle_width, + sensor_configuration_.configuration_vehicle_height, + sensor_configuration_.configuration_vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceRosWrapper::SetNewRadarParametersRequestCallback( - [[maybe_unused]] const std::shared_ptr request, - [[maybe_unused]] const std::shared_ptr response) +void ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) { std::scoped_lock lock(mtx_config_); - hw_interface_.SetRadarParameters( - sensor_configuration_.new_radar_maximum_distance, - sensor_configuration_.new_radar_frequency_slot, sensor_configuration_.new_radar_cycle_time, - sensor_configuration_.new_radar_time_slot, sensor_configuration_.new_radar_country_code, - sensor_configuration_.new_radar_powersave_standstill); + auto result = hw_interface_.SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); } std::vector @@ -537,24 +478,18 @@ ContinentalARS548HwInterfaceRosWrapper::updateParameters() rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), rclcpp::Parameter("data_port", sensor_configuration_.data_port), rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), - rclcpp::Parameter("new_sensor_ip", sensor_configuration_.new_sensor_ip), rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), rclcpp::Parameter( "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), - rclcpp::Parameter("new_plug_orientation", sensor_configuration_.new_plug_orientation), - rclcpp::Parameter("new_vehicle_length", sensor_configuration_.new_vehicle_length), - rclcpp::Parameter("new_vehicle_width", sensor_configuration_.new_vehicle_width), - rclcpp::Parameter("new_vehicle_height", sensor_configuration_.new_vehicle_height), - rclcpp::Parameter("new_vehicle_wheelbase", sensor_configuration_.new_vehicle_wheelbase), rclcpp::Parameter( - "new_radar_maximum_distance", sensor_configuration_.new_radar_maximum_distance), - rclcpp::Parameter("new_radar_frequency_slot", sensor_configuration_.new_radar_frequency_slot), - rclcpp::Parameter("new_radar_cycle_time", sensor_configuration_.new_radar_cycle_time), - rclcpp::Parameter("new_radar_time_slot", sensor_configuration_.new_radar_time_slot), - rclcpp::Parameter("new_radar_country_code", sensor_configuration_.new_radar_country_code), + "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), + rclcpp::Parameter( + "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), + rclcpp::Parameter( + "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), rclcpp::Parameter( - "new_radar_powersave_standstill", sensor_configuration_.new_radar_powersave_standstill)}); + "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); return results; } From f1e177964608cda122329e2926b5fb6cc6732b2c Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 7 Mar 2024 16:12:40 +0900 Subject: [PATCH 29/42] chore: added documentation regarding the multi hw interface Signed-off-by: Kenzo Lobos-Tsunekawa --- .../multi_continental_ars548_hw_interface_ros_wrapper.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp index 41e34a587..21355a906 100644 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -64,6 +64,10 @@ bool get_param(const std::vector & p, const std::string & nam } /// @brief Hardware interface ros wrapper of continental radar ethernet driver +/// NOTE: this is a temporal class that acts as a single hw interface for multiple devices +/// The reason behind this is a not-so-efficient multicasting and package processing when having N +/// interfaces for N devices If we end up having problems because of that we may switch to this +/// implementation. Otherwise this implementation will be removed at a later date class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceWrapperBase { From bab11b3c1b6d1375fefd9b47e8c8b5eae77498de Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 8 Mar 2024 10:22:34 +0900 Subject: [PATCH 30/42] chore: reverted pre-commit related changes (it will not pass pre-commit now though) Signed-off-by: Kenzo Lobos-Tsunekawa --- .../nebula_common/hesai/hesai_common.hpp | 48 ++-- .../include/nebula_common/nebula_common.hpp | 14 - .../robosense/robosense_common.hpp | 21 +- .../velodyne/velodyne_common.hpp | 16 -- .../nebula_hw_interface_base.hpp | 17 +- .../hesai_hw_interface.hpp | 73 +++--- .../robosense_hw_interface.hpp | 20 -- .../velodyne_hw_interface.hpp | 16 -- .../hesai_hw_interface.cpp | 240 +++++++++--------- .../robosense_hw_interface.cpp | 17 +- .../velodyne_hw_interface.cpp | 14 - .../common/nebula_driver_ros_wrapper_base.hpp | 15 -- .../nebula_hw_interface_ros_wrapper_base.hpp | 16 +- ...nental_ars548_hw_interface_ros_wrapper.hpp | 1 - .../hesai/hesai_decoder_ros_wrapper.hpp | 20 +- .../hesai/hesai_hw_interface_ros_wrapper.hpp | 19 +- .../hesai/hesai_hw_monitor_ros_wrapper.hpp | 23 +- .../robosense_decoder_ros_wrapper.hpp | 17 -- .../robosense_hw_interface_ros_wrapper.hpp | 24 +- .../robosense_hw_monitor_ros_wrapper.hpp | 20 -- .../velodyne_hw_interface_ros_wrapper.hpp | 17 -- nebula_ros/package.xml | 1 - .../src/hesai/hesai_decoder_ros_wrapper.cpp | 237 ++++++++--------- .../hesai/hesai_hw_interface_ros_wrapper.cpp | 72 ++---- .../hesai/hesai_hw_monitor_ros_wrapper.cpp | 45 +--- .../robosense_decoder_ros_wrapper.cpp | 19 +- .../robosense_hw_interface_ros_wrapper.cpp | 16 -- .../robosense_hw_monitor_ros_wrapper.cpp | 18 +- .../velodyne_hw_interface_ros_wrapper.cpp | 25 +- 29 files changed, 323 insertions(+), 778 deletions(-) diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 028a71f2d..58df85a3d 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -1,17 +1,3 @@ -// Copyright 2023 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_HESAI_COMMON_H #define NEBULA_HESAI_COMMON_H @@ -22,10 +8,7 @@ #include #include #include -#include #include -#include -#include namespace nebula { namespace drivers @@ -108,10 +91,9 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase line_ss >> laser_id >> sep >> elevation >> sep >> azimuth; elev_angle_map[laser_id - 1] = elevation; azimuth_offset_map[laser_id - 1] = azimuth; - // std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << - // azimuth << std::endl; +// std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl; } - // std::cout << "LoadFromString fin" << std::endl; +// std::cout << "LoadFromString fin" << std::endl; return Status::OK; } @@ -140,15 +122,14 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString( - const std::string & calibration_file, const std::string & calibration_string) + inline nebula::Status SaveFileFromString(const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); if (!ofs) { return Status::CANNOT_SAVE_FILE; } ofs << calibration_string; - // std::cout << calibration_string << std::endl; +// std::cout << calibration_string << std::endl; ofs.close(); return Status::OK; @@ -181,8 +162,9 @@ struct HesaiCorrection inline nebula::Status LoadFromBinary(const std::vector & buf) { size_t index; - for (index = 0; index < buf.size() - 1; index++) { - if (buf[index] == 0xee && buf[index + 1] == 0xff) break; + for (index = 0; index < buf.size()-1; index++) { + if(buf[index]==0xee && buf[index+1]==0xff) + break; } delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff)); versionMajor = buf[index + 2] & 0xff; @@ -314,7 +296,7 @@ struct HesaiCorrection // int cnt = 0; while (!ifs.eof()) { unsigned char c; - ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); + ifs.read((char *)&c, sizeof(unsigned char)); buf.emplace_back(c); } LoadFromBinary(buf); @@ -327,8 +309,7 @@ struct HesaiCorrection /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveFileFromBinary( - const std::string & correction_file, const std::vector & buf) + inline nebula::Status SaveFileFromBinary(const std::string & correction_file, const std::vector & buf) { std::cerr << "Saving in:" << correction_file << "\n"; std::ofstream ofs(correction_file, std::ios::trunc | std::ios::binary); @@ -338,20 +319,21 @@ struct HesaiCorrection } std::cerr << "Writing start...." << buf.size() << "\n"; bool sop_received = false; - for (const auto & byte : buf) { + for (const auto &byte : buf) { if (!sop_received) { if (byte == 0xEE) { std::cerr << "SOP received....\n"; sop_received = true; } } - if (sop_received) { + if(sop_received) { ofs << byte; } } std::cerr << "Closing file\n"; ofs.close(); - if (sop_received) return Status::OK; + if(sop_received) + return Status::OK; return Status::INVALID_CALIBRATION_FILE; } @@ -475,8 +457,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode case SensorModel::HESAI_PANDARAT128: if (return_mode == ReturnMode::LAST) return 0; if (return_mode == ReturnMode::STRONGEST) return 1; - if (return_mode == ReturnMode::DUAL_LAST_STRONGEST || return_mode == ReturnMode::DUAL) - return 2; + if (return_mode == ReturnMode::DUAL_LAST_STRONGEST + || return_mode == ReturnMode::DUAL) return 2; if (return_mode == ReturnMode::FIRST) return 3; if (return_mode == ReturnMode::DUAL_LAST_FIRST) return 4; if (return_mode == ReturnMode::DUAL_FIRST_STRONGEST) return 5; diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index ca0b99adb..1b9253b76 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -1,17 +1,3 @@ -// Copyright 2023 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_COMMON_H #define NEBULA_COMMON_H diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index 8a09a5978..1984642dc 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -1,17 +1,3 @@ -// Copyright 2023 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" @@ -23,7 +9,6 @@ #include #include #include -#include #include namespace nebula @@ -203,10 +188,10 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase void CreateCorrectedChannels() { - for (auto & correction : calibration) { + for(auto& correction : calibration) { uint16_t channel = 0; - for (const auto & compare : calibration) { - if (compare.elevation < correction.elevation) ++channel; + for(const auto& compare:calibration) { + if(compare.elevation < correction.elevation) ++channel; } correction.channel = channel; } diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index b8c6fb5f1..b429e7b53 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -1,17 +1,3 @@ -// Copyright 2023 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_VELODYNE_COMMON_H #define NEBULA_VELODYNE_COMMON_H @@ -21,8 +7,6 @@ #include #include -#include - namespace nebula { namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 9c73e0ac4..5b46415e7 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -1,25 +1,10 @@ -// Copyright 2023 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_HW_INTERFACE_BASE_H #define NEBULA_HW_INTERFACE_BASE_H -#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "boost_udp_driver/udp_driver.hpp" -#include #include #include #include diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index a1f646cac..77a79c3cf 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -1,17 +1,3 @@ -// 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_HESAI_HW_INTERFACE_H #define NEBULA_HESAI_HW_INTERFACE_H // Have to define macros to silence warnings about deprecated headers being used by @@ -24,13 +10,13 @@ #if (BOOST_VERSION / 100 == 1074) // Boost 1.74 #define BOOST_ALLOW_DEPRECATED_HEADERS #endif -#include "boost_tcp_driver/http_client_driver.hpp" -#include "boost_tcp_driver/tcp_driver.hpp" -#include "boost_udp_driver/udp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/hesai/hesai_status.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp" +#include "boost_tcp_driver/http_client_driver.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" +#include "boost_udp_driver/udp_driver.hpp" #include @@ -43,8 +29,6 @@ #include #include -#include -#include namespace nebula { @@ -93,15 +77,12 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117; const uint16_t MTU_SIZE = 1500; -const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2 -const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0 -const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2 -const int PTP_LOG_ANNOUNCE_INTERVAL = - 1; // Time interval between Announce messages, in units of log seconds (default: 1) -const int PTP_SYNC_INTERVAL = - 1; // Time interval between Sync messages, in units of log seconds (default: 1) -const int PTP_LOG_MIN_DELAY_INTERVAL = 0; // Minimum permitted mean time between Delay_Req - // messages, in units of log seconds (default: 0) +const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2 +const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0 +const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2 +const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1) +const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1) +const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0) const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0; const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1; @@ -395,7 +376,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) /// @param target_tcp_driver TcpDriver used /// @return Resulting status - Status syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); + Status syncGetInventory( + std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver); /// @brief Getting data with PTC_COMMAND_GET_INVENTORY_INFO (sync) /// @param ctx IO Context used /// @param callback Callback function for received HesaiInventory @@ -788,11 +770,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase /// @return Resulting status Status GetLidarRange(bool with_run = true); - Status SetClockSource( - std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, - bool with_run); - Status SetClockSource( - std::shared_ptr ctx, int clock_source, bool with_run); + Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run); + Status SetClockSource(std::shared_ptr ctx, int clock_source, bool with_run); Status SetClockSource(int clock_source, bool with_run = true); /// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG @@ -937,15 +916,27 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus SetSpinSpeedAsyncHttp(uint16_t rpm); HesaiStatus SetPtpConfigSyncHttp( - std::shared_ptr ctx, int profile, int domain, int network, - int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval); - HesaiStatus SetPtpConfigSyncHttp( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, + std::shared_ptr ctx, + int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, int logMinDelayReqInterval); + HesaiStatus SetPtpConfigSyncHttp(int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelayReqInterval); HesaiStatus SetSyncAngleSyncHttp( - std::shared_ptr ctx, int enable, int angle); + std::shared_ptr ctx, + int enable, + int angle); HesaiStatus SetSyncAngleSyncHttp(int enable, int angle); + + /// @brief Getting lidar_monitor via HTTP API /// @param ctx IO Context /// @param str_callback Callback function for received string @@ -976,8 +967,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase HesaiStatus CheckAndSetConfig(); /// @brief Convert to model in Hesai protocol from nebula::drivers::SensorModel - /// @param model - /// @return + /// @param model + /// @return int NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model); /// @brief Set target model number (for proper use of HTTP and TCP according to the support of the diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index 38db8f0c6..75cb21e65 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #pragma once // Have to define macros to silence warnings about deprecated headers being used by @@ -37,10 +21,6 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" -#include -#include -#include - namespace nebula { namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index c6c1311f4..fcb907a15 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -1,17 +1,3 @@ -// 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_VELODYNE_HW_INTERFACE_H #define NEBULA_VELODYNE_HW_INTERFACE_H @@ -40,8 +26,6 @@ #include #include -#include -#include namespace nebula { diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 96318ad10..8de67d9f8 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -1,17 +1,3 @@ -// 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_hesai/hesai_hw_interface.hpp" //#define WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -40,11 +26,13 @@ HesaiHwInterface::~HesaiHwInterface() #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: HesaiHwInterface::~HesaiHwInterface()" << std::endl; #endif - if (tcp_driver_) { + if(tcp_driver_) + { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_ is available" << std::endl; #endif - if (tcp_driver_ && tcp_driver_->isOpen()) { + if(tcp_driver_ && tcp_driver_->isOpen()) + { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_->close();" << std::endl; #endif @@ -57,11 +45,13 @@ HesaiHwInterface::~HesaiHwInterface() std::cout << ".......................ed: if(tcp_driver_)" << std::endl; #endif } - if (tcp_driver_s_) { + if(tcp_driver_s_) + { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................tcp_driver_s_ is available" << std::endl; #endif - if (tcp_driver_s_->isOpen()) { + if(tcp_driver_s_->isOpen()) + { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << ".......................st: tcp_driver_s_->close();" << std::endl; #endif @@ -268,9 +258,9 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) #endif if (!tcp_driver_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_->open()" << std::endl; + std::cout << "!tcp_driver_->open()" << std::endl; #endif - // tcp_driver_->close(); +// tcp_driver_->close(); tcp_driver_->closeSync(); return Status::ERROR_1; } @@ -281,9 +271,9 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) PandarTcpCommandPort); if (!tcp_driver_s_->open()) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "!tcp_driver_s_->open()" << std::endl; + std::cout << "!tcp_driver_s_->open()" << std::endl; #endif - // tcp_driver_s_->close(); +// tcp_driver_s_->close(); tcp_driver_s_->closeSync(); return Status::ERROR_1; } @@ -291,11 +281,11 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor) return Status::OK; } -Status HesaiHwInterface::FinalizeTcpDriver() -{ +Status HesaiHwInterface::FinalizeTcpDriver() { try { tcp_driver_->close(); - } catch (std::exception & e) { + } + catch(std::exception &e) { PrintError("Error while finalizing the TcpDriver"); return Status::UDP_CONNECTION_ERROR; } @@ -333,7 +323,7 @@ Status HesaiHwInterface::syncGetLidarCalibration( target_tcp_driver->syncSendReceiveHeaderPayload( buf_vec, - [this]([[maybe_unused]] const std::vector & received_bytes) { + [this]([[maybe_unused]]const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -437,7 +427,7 @@ Status HesaiHwInterface::GetLidarCalibration( target_tcp_driver->asyncSendReceiveHeaderPayload( buf_vec, - [this]([[maybe_unused]] const std::vector & received_bytes) { + [this]([[maybe_unused]]const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -1062,11 +1052,7 @@ Status HesaiHwInterface::syncGetInventory( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback) { - std::cout - << "Status " - "HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> " - "target_tcp_driver, std::function callback)" - << std::endl; + std::cout << "Status HesaiHwInterface::syncGetInventory(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, std::function callback)" << std::endl; std::vector buf_vec; int len = 0; buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH); @@ -1083,13 +1069,13 @@ Status HesaiHwInterface::syncGetInventory( } // It doesn't work even when the sensor is not connected... - if (!target_tcp_driver) { + if(!target_tcp_driver){ PrintError("!target_tcp_driver"); return Status::ERROR_1; } // It doesn't work even when the sensor is not connected... - if (!target_tcp_driver->isOpen()) { + if(!target_tcp_driver->isOpen()){ PrintError("!target_tcp_driver->isOpen()"); return Status::ERROR_1; } @@ -1107,8 +1093,7 @@ Status HesaiHwInterface::syncGetInventory( #endif PrintDebug(received_bytes); }, - [this, target_tcp_driver, - callback]([[maybe_unused]] const std::vector & received_bytes) { + [this, target_tcp_driver, callback]([[maybe_unused]]const std::vector & received_bytes) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE for (const auto & b : received_bytes) { std::cout << static_cast(b) << ", "; @@ -1169,8 +1154,8 @@ Status HesaiHwInterface::syncGetInventory( Status HesaiHwInterface::syncGetInventory( std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver) { - return syncGetInventory( - target_tcp_driver, [this](HesaiInventory & result) { std::cout << result << std::endl; }); + return syncGetInventory(target_tcp_driver, + [this](HesaiInventory & result) { std::cout << result << std::endl; }); } Status HesaiHwInterface::syncGetInventory( std::shared_ptr ctx, @@ -1187,7 +1172,9 @@ Status HesaiHwInterface::syncGetInventory(std::shared_ptr callback) { return syncGetInventory( - tcp_driver_, [this, callback](HesaiInventory & result) { callback(result); }); + tcp_driver_, [this, callback](HesaiInventory & result) { + callback(result); + }); } Status HesaiHwInterface::GetInventory( @@ -2739,15 +2726,9 @@ Status HesaiHwInterface::GetLidarMonitor(bool with_run) [this](HesaiLidarMonitor & result) { std::cout << result << std::endl; }, with_run); } -void HesaiHwInterface::IOContextRun() -{ - m_owned_ctx->run(); -} +void HesaiHwInterface::IOContextRun() { m_owned_ctx->run(); } -std::shared_ptr HesaiHwInterface::GetIOContext() -{ - return m_owned_ctx; -} +std::shared_ptr HesaiHwInterface::GetIOContext() { return m_owned_ctx; } HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( std::shared_ptr ctx, @@ -2777,10 +2758,7 @@ HesaiStatus HesaiHwInterface::GetHttpClientDriverOnce( return st; } -void HesaiHwInterface::str_cb(const std::string & str) -{ - PrintInfo(str); -} +void HesaiHwInterface::str_cb(const std::string & str) { PrintInfo(str); } HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp( std::shared_ptr ctx, uint16_t rpm) @@ -2819,8 +2797,13 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm) } HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( - std::shared_ptr ctx, int profile, int domain, int network, - int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval) + std::shared_ptr ctx, + int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelayReqInterval) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); @@ -2828,47 +2811,53 @@ HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( return st; } - auto response = - hcd->get((boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" - "\"Profile\": %d," - "\"Domain\": %d," - "\"Network\": %d," - "\"LogAnnounceInterval\": %d," - "\"LogSyncInterval\": %d," - "\"LogMinDelayReqInterval\": %d," - "\"tsn_switch\": %d" - "}") % - profile % domain % network % logAnnounceInterval % logSyncInterval % - logMinDelayReqInterval % 0) - .str()); + auto response = hcd->get( + (boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \ + "\"Profile\": %d," \ + "\"Domain\": %d," \ + "\"Network\": %d," \ + "\"LogAnnounceInterval\": %d," \ + "\"LogSyncInterval\": %d," \ + "\"LogMinDelayReqInterval\": %d," \ + "\"tsn_switch\": %d" \ + "}") + % profile % domain % network % logAnnounceInterval % logSyncInterval % logMinDelayReqInterval % 0 + ).str()); ctx->run(); PrintInfo(response); return Status::OK; } -HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp( - int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval, - int logMinDelayReqInterval) +HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile, + int domain, + int network, + int logAnnounceInterval, + int logSyncInterval, + int logMinDelayReqInterval) { - return SetPtpConfigSyncHttp( - std::make_shared(), profile, domain, network, logAnnounceInterval, - logSyncInterval, logMinDelayReqInterval); + return SetPtpConfigSyncHttp(std::make_shared(), + profile, + domain, + network, + logAnnounceInterval, + logSyncInterval, + logMinDelayReqInterval); } HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( - std::shared_ptr ctx, int enable, int angle) + std::shared_ptr ctx, + int enable, + int angle) { std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd; auto st = GetHttpClientDriverOnce(ctx, hcd); if (st != Status::OK) { return st; } - auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" - "\"sync\": %d," - "\"syncAngle\": %d" - "}") % - enable % angle) - .str(); + auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \ + "\"sync\": %d," \ + "\"syncAngle\": %d" \ + "}") % enable % angle).str(); PrintInfo(tmp_str); auto response = hcd->get(tmp_str); ctx->run(); @@ -2878,7 +2867,9 @@ HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp( HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle) { - return SetSyncAngleSyncHttp(std::make_shared(), enable, angle); + return SetSyncAngleSyncHttp(std::make_shared(), + enable, + angle); } HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp( @@ -2928,9 +2919,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { - PrintError( - "Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual " - "mode."); + PrintError("Invalid Return Mode for this sensor. Please check your settings. Falling back to Dual mode."); return_mode_int = 2; } SetReturnMode(return_mode_int); @@ -2991,7 +2980,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( t.join(); } - if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128) { + if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){ set_flg = true; auto sync_angle = static_cast(hesai_config.sync_angle / 100); auto scan_phase = static_cast(sensor_configuration->scan_phase); @@ -3003,7 +2992,9 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync)); PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle)); PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase)); - std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); }); + std::thread t([this, sync_flg, scan_phase] { + SetSyncAngle(sync_flg, scan_phase); + }); t.join(); } @@ -3011,18 +3002,28 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( PrintInfo("Trying to set Clock source to PTP"); SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE); PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP"); - SetPtpConfig( - PTP_PROFILE, PTP_DOMAIN_ID, PTP_NETWORK_TRANSPORT, PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); + SetPtpConfig(PTP_PROFILE, + PTP_DOMAIN_ID, + PTP_NETWORK_TRANSPORT, + PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, + PTP_LOG_MIN_DELAY_INTERVAL + ); }); t.join(); - } else { // AT128 only supports PTP setup via HTTP + } + else { //AT128 only supports PTP setup via HTTP PrintInfo("Trying to set SyncAngle via HTTP"); - SetSyncAngleSyncHttp(1, static_cast(sensor_configuration->scan_phase)); + SetSyncAngleSyncHttp(1, + static_cast(sensor_configuration->scan_phase)); PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP via HTTP"); - SetPtpConfigSyncHttp( - PTP_PROFILE, PTP_DOMAIN_ID, PTP_NETWORK_TRANSPORT, PTP_LOG_ANNOUNCE_INTERVAL, - PTP_SYNC_INTERVAL, PTP_LOG_MIN_DELAY_INTERVAL); + SetPtpConfigSyncHttp(PTP_PROFILE, + PTP_DOMAIN_ID, + PTP_NETWORK_TRANSPORT, + PTP_LOG_ANNOUNCE_INTERVAL, + PTP_SYNC_INTERVAL, + PTP_LOG_MIN_DELAY_INTERVAL); + } #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE @@ -3179,12 +3180,13 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig() */ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel model) { + switch (model) { case SensorModel::HESAI_PANDAR40P: return 0; case SensorModel::HESAI_PANDAR64: return 2; - case SensorModel::HESAI_PANDARQT64: // check required + case SensorModel::HESAI_PANDARQT64://check required return 15; case SensorModel::HESAI_PANDAR40M: return 17; @@ -3194,9 +3196,9 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return 32; case SensorModel::HESAI_PANDARXT32M: return 38; - case SensorModel::HESAI_PANDAR128_E3X: // check required + case SensorModel::HESAI_PANDAR128_E3X://check required return 40; - case SensorModel::HESAI_PANDAR128_E4X: // check required + case SensorModel::HESAI_PANDAR128_E4X://check required return 40; case SensorModel::HESAI_PANDARAT128: return 48; @@ -3205,14 +3207,8 @@ int HesaiHwInterface::NebulaModelToHesaiModelNo(nebula::drivers::SensorModel mod return -1; } } -void HesaiHwInterface::SetTargetModel(int model) -{ - target_model_no = model; -} -void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) -{ - target_model_no = NebulaModelToHesaiModelNo(model); -} +void HesaiHwInterface::SetTargetModel(int model) { target_model_no = model; } +void HesaiHwInterface::SetTargetModel(nebula::drivers::SensorModel model) { target_model_no = NebulaModelToHesaiModelNo(model); } bool HesaiHwInterface::UseHttpSetSpinRate(int model) { @@ -3252,10 +3248,7 @@ bool HesaiHwInterface::UseHttpSetSpinRate(int model) break; } } -bool HesaiHwInterface::UseHttpSetSpinRate() -{ - return UseHttpSetSpinRate(target_model_no); -} +bool HesaiHwInterface::UseHttpSetSpinRate() { return UseHttpSetSpinRate(target_model_no); } bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) { switch (model) { @@ -3294,20 +3287,17 @@ bool HesaiHwInterface::UseHttpGetLidarMonitor(int model) break; } } -bool HesaiHwInterface::UseHttpGetLidarMonitor() -{ - return UseHttpGetLidarMonitor(target_model_no); -} +bool HesaiHwInterface::UseHttpGetLidarMonitor() { return UseHttpGetLidarMonitor(target_model_no); } bool HesaiHwInterface::CheckLock( std::timed_mutex & tm, int & fail_cnt, const int & fail_cnt_max, std::string name) { if (wl) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - // std::chrono::time_point start_time = std::chrono::steady_clock::now(); +// std::chrono::time_point start_time = std::chrono::steady_clock::now(); std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now(); std::time_t stt = std::chrono::system_clock::to_time_t(start_time); - const std::tm * system_local_time = std::localtime(&stt); + const std::tm* system_local_time = std::localtime(&stt); std::cout << "try_lock_for: start at " << std::put_time(system_local_time, "%c") << std::endl; /* std::chrono::system_clock::time_point test_time = std::chrono::system_clock::now(); @@ -3315,21 +3305,18 @@ bool HesaiHwInterface::CheckLock( const std::tm* local_time = std::localtime(&tst); std::cerr << "try_lock_for: test at " << std::put_time(local_time, "%c") << std::endl; auto dur_test = test_time - start_time; - std::cerr << "test: " << name << " : " << - std::chrono::duration_cast(dur_test).count() << std::endl; + std::cerr << "test: " << name << " : " << std::chrono::duration_cast(dur_test).count() << std::endl; */ #endif if (!tm.try_lock_for(std::chrono::milliseconds(timeout_))) { #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE - std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" - << std::endl; - std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); - std::time_t edt = std::chrono::system_clock::to_time_t(end_time); - const std::tm * end_local_time = std::localtime(&edt); - std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; - auto dur = end_time - start_time; - std::cerr << "timeout: " << name << " : " - << std::chrono::duration_cast(dur).count() << std::endl; + std::cout << "if (!tm.try_lock_for(std::chrono::milliseconds(" << timeout_ << "))) {" << std::endl; + std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now(); + std::time_t edt = std::chrono::system_clock::to_time_t(end_time); + const std::tm* end_local_time = std::localtime(&edt); + std::cerr << "try_lock_for: end at " << std::put_time(end_local_time, "%c") << std::endl; + auto dur = end_time - start_time; + std::cerr << "timeout: " << name << " : " << std::chrono::duration_cast(dur).count() << std::endl; #endif PrintDebug("timeout: " + name); fail_cnt++; @@ -3343,7 +3330,8 @@ bool HesaiHwInterface::CheckLock( tcp_driver_s_->closeSync(); tcp_driver_s_->open(); } - } else { + } + else { return true; } return false; diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index 05a3bdb6a..d5e3e08bb 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #include "nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp" namespace nebula { @@ -149,6 +133,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() info_udp_driver_->receiver()->asyncReceive( std::bind(&RobosenseHwInterface::ReceiveInfoPacketCallback, this, std::placeholders::_1)); + } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 30f6a493a..8a8bfefbe 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -1,17 +1,3 @@ -// 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_velodyne/velodyne_hw_interface.hpp" namespace nebula diff --git a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp index 694928ce4..8ca27b175 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_driver_ros_wrapper_base.hpp @@ -1,17 +1,3 @@ -// Copyright 2023 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_DRIVER_WRAPPER_BASE_H #define NEBULA_DRIVER_WRAPPER_BASE_H @@ -24,7 +10,6 @@ #include -#include #include #include diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp index 24c693978..d9726a821 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp @@ -1,17 +1,3 @@ -// 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_HW_INTERFACE_WRAPPER_BASE_H #define NEBULA_HW_INTERFACE_WRAPPER_BASE_H @@ -56,7 +42,7 @@ class NebulaHwInterfaceWrapperBase virtual Status InitializeHwInterface( const drivers::SensorConfigurationBase & sensor_configuration) = 0; // void SendDataPacket(const std::vector &buffer); // Ideally this will be - // implemented as specific functions, GetFanStatus, GetEchoMode + // implemented as specific funtions, GetFanStatus, GetEchoMode /// @brief Enable sensor setup during initialization and set_parameters_callback bool setup_sensor; diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp index 21355a906..1edf7bc04 100644 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -29,7 +29,6 @@ #include #include #include -#include #include diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp index 0a6118df4..52518fcd0 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp @@ -1,17 +1,3 @@ -// 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_HesaiDriverRosWrapper_H #define NEBULA_HesaiDriverRosWrapper_H @@ -27,13 +13,11 @@ #include #include +#include + #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" -#include -#include -#include - namespace nebula { namespace ros diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index c0598fc69..1802f87b2 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -1,25 +1,11 @@ -// 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_HesaiHwInterfaceRosWrapper_H #define NEBULA_HesaiHwInterfaceRosWrapper_H -#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" #include #include @@ -30,11 +16,8 @@ #include -#include #include -#include #include -#include namespace nebula { diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp index f3ba88b24..857f638cd 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_monitor_ros_wrapper.hpp @@ -1,40 +1,25 @@ -// 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_HesaiHwMonitorRosWrapper_H #define NEBULA_HesaiHwMonitorRosWrapper_H -#include "boost_tcp_driver/tcp_driver.hpp" #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" #include "nebula_ros/common/nebula_hw_monitor_ros_wrapper_base.hpp" +#include "boost_tcp_driver/tcp_driver.hpp" #include #include #include #include +#include + +#include #include #include #include -#include -#include -#include #include -#include namespace nebula { diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp index 81315a971..553d653ab 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #pragma once #include "nebula_common/nebula_common.hpp" @@ -34,7 +18,6 @@ #include "robosense_msgs/msg/robosense_scan.hpp" #include -#include namespace nebula { diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp index 19aa36f54..74e33231e 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #pragma once #include "nebula_common/nebula_common.hpp" @@ -28,11 +12,6 @@ #include "robosense_msgs/msg/robosense_packet.hpp" #include "robosense_msgs/msg/robosense_scan.hpp" -#include -#include -#include -#include - namespace nebula { namespace ros @@ -102,8 +81,7 @@ class RobosenseHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterf /// @brief Callback for receiving RobosensePacket /// @param difop_buffer Received DIFOP packet - void ReceiveInfoDataCallback( - std::unique_ptr difop_buffer); + void ReceiveInfoDataCallback(std::unique_ptr difop_buffer); }; } // namespace ros diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp index 80b8ff257..2ecdc0b4f 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #pragma once #include "boost_tcp_driver/tcp_driver.hpp" @@ -34,12 +18,8 @@ #include #include -#include -#include #include -#include #include -#include namespace nebula { diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp index 6576332b5..5a0e094c3 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_hw_interface_ros_wrapper.hpp @@ -1,17 +1,3 @@ -// 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_VelodyneHwInterfaceRosWrapper_H #define NEBULA_VelodyneHwInterfaceRosWrapper_H @@ -30,10 +16,7 @@ #include #include -#include #include -#include -#include namespace nebula { diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 8314fc91f..87bc000c9 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -25,7 +25,6 @@ rclcpp rclcpp_components robosense_msgs - std_srvs tf2_eigen tf2_geometry_msgs tf2_ros diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 49045f5f2..573de5696 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp @@ -1,17 +1,3 @@ -// 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/hesai/hesai_decoder_ros_wrapper.hpp" namespace nebula @@ -78,7 +64,7 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; - } + }; if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { @@ -102,8 +88,8 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( - pointcloud, std::get<1>(pointcloud_ts)); + const auto autoware_ex_cloud = + nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = @@ -112,8 +98,7 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback( } auto runtime = std::chrono::high_resolution_clock::now() - t_start; - RCLCPP_DEBUG( - get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); + RCLCPP_DEBUG(get_logger(), "PROFILING {'d_total': %lu, 'n_out': %lu}", runtime.count(), pointcloud->size()); } void HesaiDriverRosWrapper::PublishCloud( @@ -150,10 +135,7 @@ Status HesaiDriverRosWrapper::InitializeDriver( return driver_ptr_->GetStatus(); } -Status HesaiDriverRosWrapper::GetStatus() -{ - return wrapper_status_; -} +Status HesaiDriverRosWrapper::GetStatus() { return wrapper_status_; } Status HesaiDriverRosWrapper::GetParameters( drivers::HesaiSensorConfiguration & sensor_configuration, @@ -293,53 +275,48 @@ Status HesaiDriverRosWrapper::GetParameters( hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - + bool run_local = false; RCLCPP_INFO_STREAM( - this->get_logger(), - "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip << "'"); - std::cout << "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip - << "'" << std::endl; + this->get_logger(), "Trying to acquire calibration data from sensor: '" + << sensor_configuration.sensor_ip << "'"); + std::cout << "Trying to acquire calibration data from sensor: '" << sensor_configuration.sensor_ip << "'" << std::endl; if (sensor_configuration.sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { std::string calibration_file_path_from_sensor; if (!calibration_configuration.calibration_file.empty()) { int ext_pos = calibration_configuration.calibration_file.find_last_of('.'); - calibration_file_path_from_sensor += - calibration_configuration.calibration_file.substr(0, ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(0, ext_pos); calibration_file_path_from_sensor += "_from_sensor"; - calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr( - ext_pos, calibration_configuration.calibration_file.size() - ext_pos); + calibration_file_path_from_sensor += calibration_configuration.calibration_file.substr(ext_pos, calibration_configuration.calibration_file.size() - ext_pos); } - std::future future = std::async( - std::launch::async, - [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &calibration_configuration, - &calibration_file_path_from_sensor](const std::string & str) { - auto rt = calibration_configuration.SaveFileFromString( - calibration_file_path_from_sensor, str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), - "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); - } - rt = calibration_configuration.LoadFromString(str); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); - } else { - RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); - } - }, - true); - } else { - run_local = true; - } - }); + std::future future = std::async(std::launch::async, [this, &calibration_configuration, &calibration_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + hw_interface_.GetLidarCalibrationFromSensor( + [this, &calibration_configuration, &calibration_file_path_from_sensor](const std::string & str) { + auto rt = calibration_configuration.SaveFileFromString(calibration_file_path_from_sensor, str); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromString success:" << calibration_file_path_from_sensor << "\n"); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromString failed:" << calibration_file_path_from_sensor << "\n"); + } + rt = calibration_configuration.LoadFromString(str); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromString success:" << str << "\n"); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromString failed:" << str << "\n"); + } + }, + true); + }else{ + run_local = true; + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); if (status == std::future_status::timeout) { @@ -347,32 +324,32 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), - "Acquired calibration data from sensor: '" << sensor_configuration.sensor_ip << "'"); + this->get_logger(), "Acquired calibration data from sensor: '" + << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), - "The calibration has been saved in '" << calibration_file_path_from_sensor << "'"); + this->get_logger(), "The calibration has been saved in '" + << calibration_file_path_from_sensor << "'"); } - if (run_local) { + if(run_local) { bool run_org = false; if (calibration_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); + auto cal_status = + calibration_configuration.LoadFromFile(calibration_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - } else { + }else{ RCLCPP_INFO_STREAM( - this->get_logger(), - "Load calibration data from: '" << calibration_file_path_from_sensor << "'"); + this->get_logger(), "Load calibration data from: '" + << calibration_file_path_from_sensor << "'"); } } - if (run_org) { + if(run_org) { if (calibration_configuration.calibration_file.empty()) { RCLCPP_ERROR_STREAM( - this->get_logger(), - "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), "Empty Calibration_file File: '" << calibration_configuration.calibration_file << "'"); return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = @@ -383,65 +360,55 @@ Status HesaiDriverRosWrapper::GetParameters( this->get_logger(), "Given Calibration File: '" << calibration_configuration.calibration_file << "'"); return cal_status; - } else { + }else{ RCLCPP_INFO_STREAM( - this->get_logger(), - "Load calibration data from: '" << calibration_configuration.calibration_file << "'"); + this->get_logger(), "Load calibration data from: '" + << calibration_configuration.calibration_file << "'"); } } } } - } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 + } else { // sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128 run_local = true; std::string correction_file_path_from_sensor; if (!correction_file_path.empty()) { int ext_pos = correction_file_path.find_last_of('.'); correction_file_path_from_sensor += correction_file_path.substr(0, ext_pos); correction_file_path_from_sensor += "_from_sensor"; - correction_file_path_from_sensor += - correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); + correction_file_path_from_sensor += correction_file_path.substr(ext_pos, correction_file_path.size() - ext_pos); } - std::future future = std::async( - std::launch::async, - [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { - if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { - hw_interface_.GetLidarCalibrationFromSensor( - [this, &correction_configuration, &correction_file_path_from_sensor, - &run_local](const std::vector & received_bytes) { - RCLCPP_INFO_STREAM( - get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); - auto rt = correction_configuration.SaveFileFromBinary( - correction_file_path_from_sensor, received_bytes); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), - "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "SaveFileFromBinary failed:" << correction_file_path_from_sensor - << ". Falling back to offline calibration file."); - run_local = true; - } - rt = correction_configuration.LoadFromBinary(received_bytes); - if (rt == Status::OK) { - RCLCPP_INFO_STREAM( - get_logger(), "LoadFromBinary success" - << "\n"); - run_local = false; - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "LoadFromBinary failed" - << ". Falling back to offline calibration file."); - run_local = true; - } - }); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "InitializeTcpDriver failed. Falling back to offline calibration file."); - run_local = true; - } - }); + std::future future = std::async(std::launch::async, [this, &correction_configuration, &correction_file_path_from_sensor, &run_local]() { + if (hw_interface_.InitializeTcpDriver(false) == Status::OK) { + hw_interface_.GetLidarCalibrationFromSensor( + [this, &correction_configuration, &correction_file_path_from_sensor, &run_local](const std::vector & received_bytes) { + RCLCPP_INFO_STREAM(get_logger(), "AT128 calibration size:" << received_bytes.size() << "\n"); + auto rt = correction_configuration.SaveFileFromBinary(correction_file_path_from_sensor, received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "SaveFileFromBinary success:" << correction_file_path_from_sensor << "\n"); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "SaveFileFromBinary failed:" << correction_file_path_from_sensor << ". Falling back to offline calibration file."); + run_local = true; + } + rt = correction_configuration.LoadFromBinary(received_bytes); + if(rt == Status::OK) + { + RCLCPP_INFO_STREAM(get_logger(), "LoadFromBinary success" << "\n"); + run_local = false; + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "LoadFromBinary failed" << ". Falling back to offline calibration file."); + run_local = true; + } + }); + }else{ + RCLCPP_ERROR_STREAM(get_logger(), "InitializeTcpDriver failed. Falling back to offline calibration file."); + run_local = true; + } + }); std::future_status status; status = future.wait_for(std::chrono::milliseconds(8000)); if (status == std::future_status::timeout) { @@ -449,28 +416,29 @@ Status HesaiDriverRosWrapper::GetParameters( run_local = true; } else if (status == std::future_status::ready && !run_local) { RCLCPP_INFO_STREAM( - this->get_logger(), - "Acquired correction data from sensor: '" << sensor_configuration.sensor_ip << "'"); + this->get_logger(), "Acquired correction data from sensor: '" + << sensor_configuration.sensor_ip << "'"); RCLCPP_INFO_STREAM( - this->get_logger(), - "The correction has been saved in '" << correction_file_path_from_sensor << "'"); + this->get_logger(), "The correction has been saved in '" + << correction_file_path_from_sensor << "'"); } - if (run_local) { + if(run_local) { bool run_org = false; if (correction_file_path_from_sensor.empty()) { run_org = true; } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path_from_sensor); + auto cal_status = + correction_configuration.LoadFromFile(correction_file_path_from_sensor); if (cal_status != Status::OK) { run_org = true; - } else { + }else{ RCLCPP_INFO_STREAM( - this->get_logger(), - "Load correction data from: '" << correction_file_path_from_sensor << "'"); + this->get_logger(), "Load correction data from: '" + << correction_file_path_from_sensor << "'"); } } - if (run_org) { + if(run_org) { if (correction_file_path.empty()) { RCLCPP_ERROR_STREAM( this->get_logger(), "Empty Correction File: '" << correction_file_path << "'"); @@ -482,14 +450,15 @@ Status HesaiDriverRosWrapper::GetParameters( RCLCPP_ERROR_STREAM( this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); return cal_status; - } else { + }else{ RCLCPP_INFO_STREAM( - this->get_logger(), "Load correction data from: '" << correction_file_path << "'"); + this->get_logger(), "Load correction data from: '" + << correction_file_path << "'"); } } } } - } // end AT128 + } // end AT128 // Do not use outside of this location hw_interface_.~HesaiHwInterface(); RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 6165c2607..d6de17e78 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -1,17 +1,3 @@ -// 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/hesai/hesai_hw_interface_ros_wrapper.hpp" namespace nebula @@ -37,31 +23,36 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions std::static_pointer_cast(sensor_cfg_ptr)); #if not defined(TEST_PCAP) Status rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); - if (this->retry_hw_) { + if(this->retry_hw_) + { int cnt = 0; RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); - while (rt == Status::ERROR_1) { + while(rt == Status::ERROR_1) + { cnt++; - std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Retry: " << cnt); rt = hw_interface_.InitializeTcpDriver(this->setup_sensor); } } - if (rt != Status::ERROR_1) { - try { + if(rt != Status::ERROR_1){ + try{ std::vector thread_pool{}; - thread_pool.emplace_back([this] { - hw_interface_.GetInventory( // ios, - [this](HesaiInventory & result) { - RCLCPP_INFO_STREAM(get_logger(), result); - hw_interface_.SetTargetModel(result.model); - }); - }); - for (std::thread & th : thread_pool) { - th.join(); - } - } catch (...) { + thread_pool.emplace_back([this] { + hw_interface_.GetInventory( // ios, + [this](HesaiInventory & result) { + RCLCPP_INFO_STREAM(get_logger(), result); + hw_interface_.SetTargetModel(result.model); + }); + }); + for (std::thread & th : thread_pool) { + th.join(); + } + + } + catch (...) + { std::cout << "catch (...) in parent" << std::endl; RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor..."); } @@ -69,10 +60,10 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions hw_interface_.CheckAndSetConfig(); updateParameters(); } - } else { - RCLCPP_ERROR_STREAM( - get_logger(), - "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); + } + else + { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get model from sensor... Set from config: " << sensor_cfg_ptr->sensor_model); hw_interface_.SetTargetModel(sensor_cfg_ptr->sensor_model); } #endif @@ -156,8 +147,7 @@ HesaiHwInterfaceRosWrapper::HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions StreamStart(); } -HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() -{ +HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); hw_interface_.FinalizeTcpDriver(); } @@ -170,14 +160,8 @@ Status HesaiHwInterfaceRosWrapper::StreamStart() return interface_status_; } -Status HesaiHwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status HesaiHwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} +Status HesaiHwInterfaceRosWrapper::StreamStop() { return Status::OK; } +Status HesaiHwInterfaceRosWrapper::Shutdown() { return Status::OK; } Status HesaiHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) diff --git a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp index e723376d7..82be03022 100644 --- a/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_monitor_ros_wrapper.cpp @@ -1,17 +1,3 @@ -// 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/hesai/hesai_hw_monitor_ros_wrapper.hpp" namespace nebula @@ -75,8 +61,9 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o hw_interface_.SetLogger(std::make_shared(this->get_logger())); hw_interface_.SetSensorConfiguration( std::static_pointer_cast(sensor_cfg_ptr)); - while (hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) { - std::this_thread::sleep_for(std::chrono::milliseconds(8000)); // >5000 + while(hw_interface_.InitializeTcpDriver(false) == Status::ERROR_1) + { + std::this_thread::sleep_for(std::chrono::milliseconds(8000));// >5000 } std::vector thread_pool{}; thread_pool.emplace_back([this] { @@ -102,19 +89,10 @@ HesaiHwMonitorRosWrapper::HesaiHwMonitorRosWrapper(const rclcpp::NodeOptions & o std::bind(&HesaiHwMonitorRosWrapper::paramCallback, this, std::placeholders::_1)); } -Status HesaiHwMonitorRosWrapper::MonitorStart() -{ - return interface_status_; -} +Status HesaiHwMonitorRosWrapper::MonitorStart() { return interface_status_; } -Status HesaiHwMonitorRosWrapper::MonitorStop() -{ - return Status::OK; -} -Status HesaiHwMonitorRosWrapper::Shutdown() -{ - return Status::OK; -} +Status HesaiHwMonitorRosWrapper::MonitorStop() { return Status::OK; } +Status HesaiHwMonitorRosWrapper::Shutdown() { return Status::OK; } Status HesaiHwMonitorRosWrapper::InitializeHwMonitor( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -635,12 +613,11 @@ void HesaiHwMonitorRosWrapper::HesaiCheckVoltage( } } -HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() -{ - RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); - hw_interface_.FinalizeTcpDriver(); -} + HesaiHwMonitorRosWrapper::~HesaiHwMonitorRosWrapper() { + RCLCPP_INFO_STREAM(get_logger(), "Closing TcpDriver"); + hw_interface_.FinalizeTcpDriver(); + } -RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) + RCLCPP_COMPONENTS_REGISTER_NODE(HesaiHwMonitorRosWrapper) } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index b7ab9e585..9025ab080 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -1,20 +1,5 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #include "nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp" + namespace nebula { namespace ros @@ -92,7 +77,7 @@ void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( if (pointcloud == nullptr) { RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); return; - } + }; if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { diff --git a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp index d0655d3e5..773249813 100644 --- a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #include "nebula_ros/robosense/robosense_hw_interface_ros_wrapper.hpp" namespace nebula diff --git a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp index ffaf10e62..9818d10e5 100644 --- a/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_monitor_ros_wrapper.cpp @@ -1,19 +1,3 @@ -// Copyright 2023 LeoDrive. -// -// 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. - -// Developed by LeoDrive, 2023 - #include "nebula_ros/robosense/robosense_hw_monitor_ros_wrapper.hpp" #include @@ -52,7 +36,7 @@ Status RobosenseHwMonitorRosWrapper::Shutdown() { return Status::OK; } - +// Status RobosenseHwMonitorRosWrapper::InitializeHwMonitor( const drivers::SensorConfigurationBase & sensor_configuration) { diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp index 17903764e..cbbc19e05 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -1,17 +1,3 @@ -// 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/velodyne/velodyne_hw_interface_ros_wrapper.hpp" namespace nebula @@ -76,14 +62,8 @@ Status VelodyneHwInterfaceRosWrapper::StreamStart() return interface_status_; } -Status VelodyneHwInterfaceRosWrapper::StreamStop() -{ - return Status::OK; -} -Status VelodyneHwInterfaceRosWrapper::Shutdown() -{ - return Status::OK; -} +Status VelodyneHwInterfaceRosWrapper::StreamStop() { return Status::OK; } +Status VelodyneHwInterfaceRosWrapper::Shutdown() { return Status::OK; } Status VelodyneHwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think this is needed const drivers::SensorConfigurationBase & sensor_configuration) @@ -289,6 +269,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneHwInterfaceRosWrapper::paramCal get_param(p, "rotation_speed", new_param.rotation_speed) || get_param(p, "cloud_min_angle", new_param.cloud_min_angle) || get_param(p, "cloud_max_angle", new_param.cloud_max_angle)) { // || + if (0 < sensor_model_str.length()) new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); if (0 < return_mode_str.length()) From 45dbd8b7b62fd2f5012a51801fd32e05c1cdcb00 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 8 Mar 2024 18:14:41 +0900 Subject: [PATCH 31/42] feat: added unit tests for the ars548 Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_tests/CMakeLists.txt | 3 + nebula_tests/continental/CMakeLists.txt | 20 + .../continental_ros_decoder_test_ars548.cpp | 280 + .../continental_ros_decoder_test_ars548.hpp | 90 + ...ntinental_ros_decoder_test_main_ars548.cpp | 60 + .../ars548/1708578204/1708578204.db3 | Bin 0 -> 65536 bytes .../ars548/1708578204/metadata.yaml | 26 + .../ars548/1708578204_199326753_object.yaml | 3257 ++++++++ .../1708578204_202447652_detection.yaml | 7104 +++++++++++++++++ nebula_tests/package.xml | 2 + 10 files changed, 10842 insertions(+) create mode 100644 nebula_tests/continental/CMakeLists.txt create mode 100644 nebula_tests/continental/continental_ros_decoder_test_ars548.cpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_ars548.hpp create mode 100644 nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp create mode 100644 nebula_tests/data/continental/ars548/1708578204/1708578204.db3 create mode 100644 nebula_tests/data/continental/ars548/1708578204/metadata.yaml create mode 100644 nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml create mode 100644 nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml diff --git a/nebula_tests/CMakeLists.txt b/nebula_tests/CMakeLists.txt index 4a290c6ff..ac8943890 100644 --- a/nebula_tests/CMakeLists.txt +++ b/nebula_tests/CMakeLists.txt @@ -48,11 +48,14 @@ if(BUILD_TESTING) set(NEBULA_TEST_DEPENDENCIES rclcpp rosbag2_cpp + continental_msgs + nebula_msgs pandar_msgs velodyne_msgs ) + add_subdirectory(continental) add_subdirectory(hesai) add_subdirectory(velodyne) diff --git a/nebula_tests/continental/CMakeLists.txt b/nebula_tests/continental/CMakeLists.txt new file mode 100644 index 000000000..44daf8813 --- /dev/null +++ b/nebula_tests/continental/CMakeLists.txt @@ -0,0 +1,20 @@ +# Continental ARS548 +ament_auto_add_library(continental_ros_decoder_test_ars548 SHARED +continental_ros_decoder_test_ars548.cpp + ) +ament_add_gtest(continental_ros_decoder_test_main_ars548 +continental_ros_decoder_test_main_ars548.cpp + ) +target_link_libraries(continental_ros_decoder_test_ars548 ${PCL_LIBRARIES} ${NEBULA_TEST_LIBRARIES}) + +ament_target_dependencies(continental_ros_decoder_test_main_ars548 + ${NEBULA_TEST_DEPENDENCIES} + ) +target_include_directories(continental_ros_decoder_test_main_ars548 PUBLIC + ${PROJECT_SOURCE_DIR}/src/continental + include + ) +target_link_libraries(continental_ros_decoder_test_main_ars548 + ${PCL_LIBRARIES} + continental_ros_decoder_test_ars548 + ) diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp new file mode 100644 index 000000000..676aa2bd3 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -0,0 +1,280 @@ +// 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_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 + +#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_ars548::ContinentalARS548SensorConfiguration 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_->RegisterDetectionListCallback( + std::bind(&ContinentalRosDecoderTest::DetectionListCallback, this, std::placeholders::_1)); + driver_ptr_->RegisterObjectListCallback( + std::bind(&ContinentalRosDecoderTest::ObjectListCallback, 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_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +{ + 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.frame_id = 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("frame_id", "some_sensor_frame", descriptor); + sensor_configuration.base_frame = 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(); + } + + 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::ofstream ostream(gt_path); + // ostream << msg_as_string; + // ostream.close(); +} + +void ContinentalRosDecoderTest::DetectionListCallback( + std::unique_ptr msg) +{ + 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 << "_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) +{ + 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; + + ASSERT_EQ(1, extracted_msg.packets.size()); + + auto extracted_msg_ptr = std::make_shared(extracted_msg); + driver_ptr_->ProcessPackets(extracted_msg); + } + } + } +} + +} // namespace ros +} // namespace nebula diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp new file mode 100644 index 000000000..aab813cb2 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -0,0 +1,90 @@ +// 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_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 "nebula_ros/common/nebula_driver_ros_wrapper_base.hpp" + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +class ContinentalRosDecoderTest final : public rclcpp::Node, + NebulaDriverRosWrapperBase //, 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_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + + void CheckResult(const std::string msg_as_string, const std::string & gt_path); + + void DetectionListCallback( + 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; +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalRosDecoderTestArs548_H diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp new file mode 100644 index 000000000..a3203da89 --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.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_ars548.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_ars548.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/data/continental/ars548/1708578204/1708578204.db3 b/nebula_tests/data/continental/ars548/1708578204/1708578204.db3 new file mode 100644 index 0000000000000000000000000000000000000000..1b65d1148fe26cc3d711c8f1d6804abc640c31b5 GIT binary patch literal 65536 zcmeI52UHW=+wS*}(7ULpC;|d@LB$4{lAvP6-V34uMFkYFH|*FO_AaQ{us29UK(V)@ z$KHGIijBMX`w~1H{oi%(THm_syWh&JbI4@!+i#wEX7|Z#JG5&Rl@Q?(85e)UU~<6ZfXM-q111Md4wxMHpY1>wa|_qX6}VARG3tns2~p7z@d@G4L&Boe zBk68@c>joai{SQ6lpULRG!Jduq^n0U#G_59hY|CrQuL9BvO{pyi7s+W*9sN5iK7z2 z`wosUV#ZhV!l)gUs#Z;kf>eo8)9>lnB&11ukGAcbw^p|A=FzfAw;BntL!$aoQAHny zw&~~*+No8m8o$2On6Bs}4|RA#xQD7$84^!Cj_~N%q$?FsBxKZ(e;Hpz95p&TAu2W|tZ+PAJXLIDWJFwqI&5fceAtk< z*vP2C5%I+XSDh#`LnFHt8krEzPAobCUuDH)8;|}c;l|IyhDXH3Q&qnkY9#)pB8mgw zCe{QI@%;uwM2F*Fvtk19SQUKv?OXnNP2=b(tE1u*;$r{xRcvg|IesMcKhtJ%z~q3* z0h0qJ2TTr_956Xxa=_$($pMoCCI?Io{7-VgS|%f92Mmj*Q$=k2U+gab|D^LYor}o< zlLICPOb(bFFgajyz~q3*0h0qJ2TTr_956Wm4#53?`xrw1Gi@dZOb(bFFgajyz~q3* z0h0qJ2TTr_956ZXU*^D6N6wOjhq%nJ_KS~*iI0lu?-xgZU>@fe85bLq5Y{guCLtou zFD9bzu)*PBL&EzFib#krd>0+xKi-f2@e74d+v11))NwUE2GAdN$Ht8c^r&s);h~Ng zk}x39qaNLjix?ag-ZyG+R09314*P6aT=Dl2;p)LrF%g0EGY^mWh<e|<@0 zy+t!%d(7Sf?4Z?|jD17Q)v&q(W&8A+1FUXwo)@rv{2BtAFnJkczY_~};Ytho&~9NE zw}}u8?8v(Vft}XQim~sBotn54qNZ`BmYtCG{dNY}@jpugJ11%wvbJhsL)B8YT2?(| zJNl>a^N*^e#QRTxU7i=f*pI|YO-du%FgODBu54Nr;!b~C0@(S9)frnrT-2m2A$4fn z%2s1A?%z}Y0(Mc=AYhlZ-@{lraaI$&VEU3}laO^j?*{C$OXq>zP;WeA-x3FUT7)#E zaUF*^qlO(_z5$!MIvm)Pb>|psNlL2eA7GuPg&>>m`-E@(`i-=apN?^V-bL0Gd|+`c zljBi?`InY_gNxmyE$KagO>^AH*bk(XTJ93Wo|MsjjC2H+zq6+rKQZvCv^#zju-m)U zX6zeMMolWfwa`83)eza6%2uHF@XUk2rmMR%_BAQ1CXJEZwE7t8Jt)lw_TtkdU=KT0 zWGqK2s^uk6L)LRIV6`KkZU*+R7Po*sVBZ(xx`T%9RB!sjvB;7GdeHl_Pc+6|JczNc zh?knU5w?k?H_3rjtgmBBEjLHj#Rc=aZsQm5K`9$_AK1;|NsP55)zzdP_$!umbw|C=H;!Q% zl=7EXflawFhp`_?4Yk}K;|BJ@{Hw{H(TMq=G;cE=*u@8@G4?B|s+M!e%15L@Tt(`G z{t(x^+XrB`KZU&hiqujQ_^}lcJ!)=8ANuT11J7r-IMi7=h;nx-MInE(0k&imiu-HCG@&Er`|yJsB#G_7fP2Z4!~~ud5^_?N$MEu zGvT539J1~16VTwEnGEb+-;1cBzVW*0VjteebL;r1KFgCzk9{YB)$W9P=Lcy3aalfV zJO4JutzWwb#Pv=?y@nG9S==|Ip|LIz&OPgl?6R0DaNVjdD-UrGI1OOzPZFq>nKaxgjxh=55hhA%u-4^+f)g{UrXGLIlsTVT# z72(yy0?YA$lb9#@e#1T}o>|sV)=pmmtnTuB#=a*(YI#NXOm=R{3NnbR(dWcN+LUIWULu!s+P;mj6T>{rsH=-k3axFB1*zQ=L(jK ziGSHK_A_a%mY0UO;^=+Pfz{M5IT+Y*-?k99Z{$bD%1KMLT!tDPqp-cuG;9d%u`>K{ ze_*pq$1paZw4iws&uu_Z9f+$f7{46yWQ4~VU=tqJV{8FwMe`17=(!Wiv975s0W?JE zVu7992hL4KnyKXk*%tSgLfoLU=FoO4W3*#|oxLoJ#eGZKsO64k#|a_YAE_Jg+-9!! z<=uC9Dia!fMZHNM7)wZ78W;6;%^VLbpFrC}RzFOfkpt{h&YH0wNITF=+c)iBM@IuI z)Y!BX^d?0RjC(Phu{NXw^`XuRRxaB4y~em19ru8SMI}OjP4ex?*iWQ8Wl`_$c6Wf) zUitA8;x2K*HXu9WA!EOhUTV1w#MOI@&&RkUM+5=8{FDshu6=FK*cYUSnm9li(r%h~ z6WAaNyGXbe%g>hsHfKNzV}Fv~^xPn>c5}-%$Zo3s2;)9Pe{;4^MV8PNtxm}3_{-X2 z;-UoB>%8-@p=j)NdXBwo`+w5wWZYqI8jEnv3GvhzRKr>Qb3~rMc3} zyFP!=thcnX(`A124?%S4@RCo9_G)y0;&#oIQZI#Q&YY<7=#|iZxXe<5C4>m`$v}R= zr5L_dvljf+TdoZTo~}U*hqxttM%7okS^YBk6VbMr<;r7I&PbM&krE9?^igJaC68Vg zMVq|ll1Ju7_91M1$4e_=N3!`iyYLwO@*{>C#y5)hCHzr(>%Qsy(|YfuwZeG*n%r4* z-q@eNlhHM>Zk@)WM~`*F?jw6Zx}j5MPfSg76zxc#YFwJ1W~i?Vyjqc8p7%DR+T1e4 zplhe*wHtow-Rw8=-|kn=+%#_#q+ul6VI)c)^Q5ihv2}u2Ipl@pIV~6{AdJ$*9uoFw z*-#mN}t=YndF)ITrBg{x&4i|FpKHzpZGvxPB->#mN?)?$sN}UQR8-oJmb_-k zfntA(qzfya!-axRgy{#0q?c@J6<`K=J$GJH{;w}3lyf6D@oUeAO16#_h3F9#GtJI5 z;#ZGcCYtA6A*!*27^>{o;1>i|qpw=OIJ5)1jQ-c2*bjg^Sufcq4o?3dYk z&oUJ+hLLQCktltm{~O8MODPUJvqAFt_BT*K7+n_!b)HakRCw6nj`2-^ z!7%~z`q@(h`MYaGWxs+F{9$gUcy&X95HxE*X5xq^x*8({ao)=^M0J!(FK9|fJ#6dF z@*wYZ&7PmQI-74ZI*ni6;8?~ysxmtA!w(TDhE{3AuQz{e)U@c**|b}|djc%$9SR%T&cH2+D6)zZQ_uYdvX@El>zOJ{b+oFDw<6#w?i zPJVcH#^GJlQivfd^%SokIYQsNl8-PXDoWEeArsOtlI<`Or!XepHb@>n1H~1IW>WP! zrGNsWXbNNTJvPgOo9JnsZrrz*>>t1shDs)@#B2IDR}Q?J%x^t9LHyZiH2*2Y{NR-+ zBJ7VpDlEv{iIb$umcGK&AYJpS(K|ji>5@=y#8`gLwAhT;2G!~8OWu4z_Tjm{!@ZBf z2>;ERGog4-n~^LrBT@S5sr98wesSW;u6a`Rv`R)@zvjFo^>KPXv2c?rlPkvks(vyr zHs>W%S&aI@e|xb*d3^3r!6DO9vc3C2I9fSDQ#Z&%x5#i-7=54{Q8lA8vUalyPaNjF zbZwhRLQrfIVeaj=!ob-bw9e&*(wWBO_QJ&Bi}dx|J{C4k{ia=7W)FNLBiRlkQM&Go zy;S-28*%xI6;ky!jf}c}Nf#!V{hHquCY3W75tp)-spz_o{dfNJsUAujjRn6ed^LZ$ z{CvTp^23bwZkPFuYxPod$Lrv_lyx>2Q={us+qv?IUW+ns-iYI0-k6=~sXj>z+Puwt zUL~1+&G=cus!IbjtB#I`G>l|Bj6~_vuFjA=o$85)e8QyKpX@L)Ba9hF9DYJkbX2H) zv4ioAyTQE`&NQCvrRKv%gk)8IzKb7U;)^u?%yJ<}XxDIQmn7Y%HD$%+W4jU+ony?Z zUhP6*YL*H3x)X=+YTK%s``f+wsZHwz63&4b&Up0UF9aRehcu%<`*0|w=oE&tlZ<4E z841#L`E{F0o(sN-V{7e_d_7YNBmYx+&_g!;j+;>M1LlX>yQRMW2fA1ltquV5Tw4yxO(GKTxGLt^((@IwoBKuC$T>Fai_>6>9ltkCpjU6UcvfM0=8+AqUyD=~$W!-`LE$@#A$-|duxpbVCWhA7cB)Yy~s!pmD0 z;O_VP1#R=yLT$GjcrRmk;(SaC#Z*{4GM4Yvsk$!I>lvTKc^x_x=S~d!b1U&X`YQFA z%RdRrE>6^RPrnOk7|C`RiP9$uVjKQ?qx2dbtMId5BrDgIUdpGP>nJrn-A2gl zI3Qz1_7mZu_i@R}eJ)YWq}Oam-%*&_ioIxe9N#Q(e)j~sf<&SJg_W9pciK_cRU?Jq za~<>xA|?ph4Ij1RHtHY^BiRlkQTonVp^}g4hNwwBCDj#w`CBAi?A!{k7^Hh0x)1`< zKR)whT~Rt;PS3A-S3$Y=!4&>XUX~R1GeXGpZ=b2oD=9cxAL9*K86e%@zxtOq^bXRl zeG>mA(p!|K3=qP(XPWd=8N`se!cl11&qD7VkSH9pzoMPnn>J>2VM zX)`@akCH#-lgFr}*N5f`t*?fO0~WOvbc`re?Tqr&KWzQ#9PGunZn|Cy^MY$p7jEvK{c9tH@t(_*CX zAJ4>Tr`84O%tU?E} z<~0${K_yRs}vn1 z!pO>r#&_MDS)NDNQ`ScCcP1QC&U!nIA3JD@_cc`8gxv)r=fR={u)v5$r3}%9{McNzg0a ziuYe@#IPq%6iYT(XsX2wz)r7$-srV#QdpQ2%npI9`6ARAl%gH~=)Tx+bwz&DFU^XWF-4)MuK#m;r2|a(wZQt)}mU{fCod^hp-h%7ZR$6 z8xIm8p;{l~o4}lsFHrjaN>%v7E!RlfHq;XW@?50oxJJUp?SYxghezt?48JYdS@cHN zvz-%-sp%`C;6)`D^AO2Y}|afV45Et;8l;~m)Fr6R8Qoh`{Op6j?b{DF_I-_ zB)Xn3C`$6E-%6_2tAP|-@(({H5ihp7Ui5up#pi|98zD_vJEoxYLvFVG>3d%da~ACq znr-xvW6P}@TwfT8MDf8hNf!@_00{G-*QrMdnWgqx$2gM9iQ6XxtYE3A1& z2YGq4lwC~EDhfr2rsH`VUfpGgu6oF4KDGV4MzcrkBZd=w!ub@tmxjifN5vWw5)}J7 zbU{IsWIK#R>5aboOCAxcrN%d=N@LP%7ey|TF0Pzeq3EdaNK?-E#<7LXY?OYyuA5-l z!&ypP_fF_uDP3ItE>KusrbnhrWO;tgqEABd)7?asL+keU**HwbW+|}!@y-0muRDdd z)oTcjKNB;OYVD;(HSGhxDa_tbQ~OEu+uB95x;l228OgqyktjXk)D+1hw2aim@`W@u zq@hvQuL@E)S?PSyQDIV6y77%`y4^&SeztP~e1KZq;CJrsiy5X|WZ}RZThtp7{(OJoFR|?PH~c2s=NaK!yVH?UdcNSh*40p<{2j5L zvasE{hqxgFf~XwIH?BkLoCwt zlb?hLl6FDn#Qn8M7Pos7e|hFS!=!6N#lShIwEbe~^`aXi*()Pa`f8U_lIPkLQm2zw zrHNnmvJYV^$}mC%PluL;n=qvnj$7z^p$@kQI|CPNTME|4L$XAh(!$>nb*0veR!Vj` z`xI>+kLD}C--OMQ6>XMIyA8)cX8!b55#YC4uqbm!J9WRi*eT3`pYUY@ZI;|S36<_2 zH-v2dTb#8Zlz&$n`yz~Fi5ZE~EzPS+o=_DbaD8$d!!?l03%=yARzIvR&gK!x$28K?e}i6NTHH zVSMkSyv`~lp5J#|*Jz9|oEY-1G~`#!US;UMwuz`RtW#|I+5&h+vK>aEbk!1Cx3~UF z>RC2GN~(pu9UzM8_QdLi{U&-?bdF)oIBezp?aQO|`^W0?r$dsIhtmh}Z?`^`!W|@G zbDwpYbw&To5BpjR-_z!UbZL^|oiR0i)r=WR_(4yf2o_o9d|HGuBi4N#?Sjn<5NaJc zU})>yRctnRs`fMe7gpKENO+|rO3!jKmwfl=r9Ll{q?tbw*@v(dWf-aV`(JroET3Fp zBpMC3Pe^k%kL~C@3SI$ErZB?OwiAj10KC(lJQGNVdaBl-_iVLh|sREA_9LBhBehXbOv4lwsVgrbAjt z_ojE@92OlU?XW(E(qBK-@JHpA%6@H{@=FVbNRFTM!h+XR4$bYD#Pz{KANGtw~2OqhtJ}_HhW`ex=Js0m@-(qDH5A8MzU0l zEOfnATgmJEBx&%^+0y*3yV-}Z6-k%Uum3tBF`@@_oEgHU8>PQd6!5SA9<9{+59W0< zZ%PNf(*=1!JAU(Txa^wF;#DjPq9og4BudZexl5{C=5Hzb*eYp(4EkqC8eNxCR^t_8 zhsAa`o508PkJ%No56pE-)%(+(gb9<~kBkqs64vIgQ6B2LR`h<}o9|TVf*2j%gm16g zg09EizF|yFU-d1HCJQrr$LcT3UyGI}MZvblHLxu8Ik_gxja^{TdMMH9qI2# z#~m5(mMkoLy;WLSyPde~>Sn%yn@)65?HBtGXh~Ewqe@e%u{_Q;R*akLi3XZU3c&C6H`MCqghmEK0heGgfR12CBbU++Kl`|m*`EIy@>+%Y=R+dYZWfp>S?){+CtG$Wg!_0ev#{P+6fN!YS=ksh~y%rlmMzX|=MAyA$jF*&_m!z?4 za-=0ye~pQXq>Bq)Kz#t;DV$H$8Na-$qVR{v!Cb60pWb_gGRv)2XVI+)XU(HB#J$Jf|RJm0pY1GKG(vm%gjk^Bf`r>96ijGSD$6p%X z&`ShEYgw>a5)mfM`mCz|Stb-5UMXc|L`dTgkD;x}Mt$U&P)VBq6XR}d&Y}fPU{5AG5f^Cx4Xq|zNWF402v?L}YJlJ42^8*d|frKdkPgJQ6rc zunnpx%^z1nG|S(Z>5;RHzZTb7$d05pWoXT&TYBI`VQMzk;tP63@MDAj7V3O@#pjV} z8S8tOB!;bDJcI^Q1{sD8|0qVU%+r-Cha(+Evc!x;*N^p`B~^|HlqRPYNUPi9WD^j^ zCP{o)^)WjDo1ob?&bV(8j5UmTu+5Xnbeg-pa@?Zt{MeHRrN~9?h2=LlWb}L^Xw@EX zg$I0LYkFvWdt++)dU(w7TYUKHmV%jV8$YY*hQo&(Vrho?q7*p82EzoWc4FM?2;X9ADe#kX)=5|%WKbm zd^&6V?O1qE3bGfK@#0UVLPi%iCYBigVrL1efBMudK6y6pW1)st zcIog0-lm@k$mUrfJAghp39MK#-wiYf4w|#z@3x<{fL;6UBV*r^D0Sh##vn!-ngOfv zS}_Tpe%o%RW$cmAUe6i(l|-uv9SykC(pPTYKwvW$_|F64)dX>Wnet$2g679^fNy@{T6 z*;@wVHVE`nB-eeT)cG2Koq9ilvEN97nm#hkHX+~fF0dN+6cP05E8&x#J9EMq`+|&A z6GvE`K=;BNpV-zUTjc<2@LUdYw|{uU*f(T^nm%UEHnH=u@2DZD_9Vsl8BLVg_OF4R zR&Nz!f09x3v|!W6Z&(0qkiLqq;`^*>%Djs=fz@2dW2}sfHm;_iyLQDAotYlIpms0nQgX>h$d)B@wy8G%ngAD=e>#Di%~(%)8jUR$h4g9oU>n9E_$D5 z%}->eS|+R7jCr;!6rWc+!U<2GatEt?QmBH|~II@rEph6&>o(xT}OzV6`JEzG3y5irXB6ac%dqxN@?> zn9qbMLVaL0yZiYuy(;c(ePH!_5|Ld_S-8f+=ZEuw6-tEcVH#9s)6W8%?of%b-^eoh z1i!t+uJPSA_|&*IIbX}lu}Xd%+m^kW{)~M`R?;#Ltls-sbv(BO^)85O)!_ofT@~4r zu?1ueWo1v9XQL`E!gGrmbBmQBmDNF9$sp(J2GqNnw)0{y%IX#dG{v}v;o$Ud?4%9gw7^1y$?DE1FN&0&JpXB zicY|8pW(+?3$j*C=*^z)H15^oOHhOFQ#)4ws_d+>&Dz>vIb+|EjcR#_y+kN`VRAOI zm27$;J3klV?mP*rt>lwU^vQK#MO!fw*#-(PmM2wqtFRm&^nw0EKG{rH?EqF^<_xZi z!VhT(tM91npJV&BzCP&9Cn@y0fxr5a^~+=2nXYTV2m6;Epkc$x15EEnlB$;TGB&i; zxb?=iFwpylIa{?ukS%8OnwS%T0iJ(ye2!K55ri?#kit_&f01ivac2an>ro(h@Z%IssTL9 z%(PWQTxeriUN7xl3fSDo(C@P$>Bc-Md|cZKSi!tmE4Xg1ce(+ao&_t1SdyJ;n%UTD zL4qLw%Y~-)?XjStY{(j54_kL)=jKRu0n765z>?SwX=Xf!WrI{@k7GY7s~p6&AiHTf zcA-oA(CPdtj-Wx~H@>A}M(1{_3bt_&_sBb_s~yQ6s#lrI*oqCYP8B-0i)L+r$|LeH zuqjueEwm$hsSn5=D2rt?$ZhxqMc$VtD$jAaLPADAFQ(Uv?4#w__Zo|PuOHTZf?`p3 z$m`xM`a;~DX=NE}OZLOc3xu+bE@IzabA5=!gsZ%F;i@P{CU`Q|k{nda-BClzHq7gp zuYH_Y|4ik*8}p7vDvh2Upn5~bGYzkz1HlLF!rJvggYV#lpkep1V~pj%lu#M%9ESm^yb+Zz{>fZijalaF~&ZhvBhgK;jA}Y<60@1z-mUq80Q<&(G`F|ufC^6VO}?sJc8^8jJyBK1=Oom6PR`& zLAmb_px(pv{2-tCZwW)agP=~erCm5V=RJ@4@a7rzJ^9Fd zbCM0?3l_I~$vwdG@5ey@LDjH@KWNY&-OV%&n=#b=0~NB&mJJyFAEw-|`4m^#b@`WnrfWGz{09JtY6S}%nM+1!y&JIAy?IM=c-B;cUpT~rA9Gz^F#2hy7pi*fC=I4;xN%l`yruBRK08?sNT7;8gr z(LSZ>H1l|iRttKAs%noy9o&b<@oDbB@r<=5cZ%XBW_-hQi#mBlG3fSPRd^1zMTUxP z8S6~$f?gJPThb$71GjbTr3l&=tLhi+j6T@>V5~X0N6S6vRqPD2#<&WmPgz=5HAiLpgGhkzv;27E9 z*M!CWNS@Mb0siV`H*saEO!pK5`Euxn6wr{_AM)^L^31q`lrZsQ0mS8>FFOcv6UvOo z>$blSi~E^8H@5YPtoe0;RfzAOAgjl@n4@%(9ej9U)T_wZ(FIxaO&6d~IpSa$^lT6P zX&&p(y)?4Ii>*Ac+81T8P8&4?%f>D-im~6w8|YIKdM(bXZvq>1q4rqlCysU;f^h?& zJ~1b+X}Jf_6gN|F4kBo7?n%&_IIOVG?!SY@m63Ne?*OaIex--F{G|C8fSr^(2;v?- zpTpQszoDp z&?O1a?F-Gng{=Qmh^r~*k_vif55#_LZW8pD&B<3IE4HYu09LVS(O0-`v$shQSN{|G z8$U_DG4CkympTEP5gOE<&6%j?24mZq8&{5L_({IeYm6G+{_Kw$%Cs;*A7cJsY?E`N zVT^1+zSDj!uzdgRn*S|Jr;sUIehBr9AM&v*vP<{bz_~4~kNG#ZHnMy{%6kXuJ+{IJG;A4&ZGG;9X^fR~j`Ugpt82LvSGfxscRm)% z{C3N|5clw>n~eRzIjiN~UNC>v{ywWn(nsZmn zEZu3Rnf8~7J^Ikn3s$pL<=(`(xvW+T8Ow1VMpk#RVd30ok~OUAtIAt79W-RpyHk|? z#CaB#ocfAFA{}an`bY)9qwcj_bLiXLphtOxga5)6@9+?c|vag&ko!fx8 zTFXY0(BINaaNKas?ijGy&)^#8bCqfPR(ciHpfz8;6+;Cuqnz40AE&Tn)9%w!A;pD_VV?0U87gbF6>w zIh;Wsc8^7WYpO{(8kN=$`6bT*tLgTtKh&xB4Q6<5gB%%a$N5oz8x3dIqQhzrJh!?> zuy6FBEXK`R@tCn@TrIWCth_&)OT5{sa4c(mB9hH-s~%5Z2XS+CGZ_1otE-lA79lKd z=;`wiS37S#%tNal@86B*78}f13$Bh@W&!8Me;0ZIn|WwpMQDefJ~)MXbubrh#RbrM zhociRL})eaGR9p#crVoJFU{LxT>S$USI*T}%d9xaXTv>l%%E9Y309y~z3*EE+5PsY zSD~hzcDC_f`~Hm@@2un*W2`L~LfeZv%hWn2rj%%-Y>(73G~v21G74mM{F#oQEv8;@1X{^tL z1kaJ6LGvW&5UWdqElL#DsZTGlxP+tCzXN@xo;~YQ*dD8e&a6HQwmgIVhnz7`uM@6~ zT4rYv%+9U8`3Z9hz)!9lZQn5N)zZx|uEHyd zjp>7Jd}4v!DZXGDoVf0^evrLj=T`eP_U#45v^a=sHz*#@ZESbO+HiC#9`A>0rXR&I zgEsKYDPZk;EC6=Tm=wnTWcShS`p`PUjQz+&`*=zP(82HQ5v%a@-x}=j4ev*0*FMGg z(wv&=;0{n7nmYZl53F5!q%-Ts1Ur=+h36O0nz0<$pLszik%(wfIR)Y>G(Q%A4yW0l zfZhJYma%3Wz3+{iW>NRks3F4FjrBKzog3T&_E^j{#{S@rmNHxXJ~%wzqo z;1aIQfz4=FnX%4X4ApA`zEzt&89nYa3$JsDS24iqb2>7%1V@JqB|wDsZoysDkhZih z8~+EpnBo1s>>527TaFt_>mIXX?0S$+*tgCU--}T0OIrU8adX;3Ke2#IP|Mqx-J+2N z`DvWvRy4aa0Q8osi}^BV476M3+;G|lHoL{**0yj#y$uGNgJ-2Hl|;Qae3%9sj^3X( zL%ny@FHmnv2}l&|T74_9*>2X1t-y_>ddq%eaeWsSmJP>f3y52$9o7+s0+{2m;6|xs zC7tOc0O4=%?g4RwWZM+%enW7%x_9y1vKOI-v1(aG2l^um!rwjG2iU;$#6`#|Fz@6X zf_BS>8>g1}0jqu3`~m7+-2NQve+IjC9gcbvp-*YajaSRudQ4_%V?i zjZ-C2VEYU*7+2XQM{;rJu8oBH6I#p3!dPD3_0z9Yo-`G|cfgXa~-mg43> z-NRU`2#l+lSjHdXR_@`0de6f6w1A`coSo|)WaB-b4Z#>UXreR3tvnc4h1S1=`9WK5 zF3j;zmcN~CV+Oc+m_ar8cN3ExLdgICNpU^P>+qmkW%Wz*ohg6Xy5 z7O^^*^{x3P_V0iVoT|JE`Pa|i6XNEM2xP1?mrUm?8b(uAF|`u*ffZMRkFY*OaP93_ z@8nj9W2_~&SS@olr~ecN;p;2%P($OkMwO`8jgzp0ksl#a)t@53Hi|k@9eEt;%ElkQ;E0#dYCU6vaJV zc@OHXJZ}>la|U<#s=>JPr!dx*WA~iRPP4dK>tj*FS^7``>lbwN#<9z(!TES@tJHEk zv(tpKqbGg7*C_K30O+y+`l`Rt+jSKHd~7qI-{=B{vV0~8HF zL-w7uj4jEf&~n`7Jk_9W>ly{Dpjg)!^3EXHD`2xK&qoa#;Wt&(Gwq{_KQZpjkR1?r z&}1A>A2IL8SO;!1Z38UWZw0isf3Cr}<_}$=E*abm=YX^5eP7C!;L>P65m-KV7WQK@ zyLJ2mZDGuKESouJZX>&$#&uzNUHdp872`G@Tpjw9F`5X_s~;A{SQl;^%m*>OHS!C` z(>DgRggiOaHVS=6oX1!jIt-A>OATgmOYX&Xo}W2n3-phM&cf@KlLYmz1MS+&NFkd& z7&SP?!*3Xa3{};Dom8KyR*lD(c-s+pJQ9*}0K;oQvUg{{5j04J-WDDsoypWULdn z7vi#WvrT<+Dm-otoM3Y&PVbvD!z-K7w(tBkjh?WZWjIf!En(L0rw2gaC*;@l+w34)cRH z+(CM;ie@0nY89Aw=-0u&@DV)8t1ia9Hww?~5Y>R}iA!3Dn>l7k3&=aO*W$cQ&Y4Y& zEx{e8zptQ~jn=7+M$f`?s~2|`%G;c&*oVpaT-YbbU}HFXv!3v6Vgit@v(Osy@7$*q zK|^jzO%~UV%T&uMnB_6HZTEJ_RvHWcT1D{O{CCJsuESUdPD95}$OiYsaT)J3SO(>I zVP_mq=WZLxSZhv8+j;LPG%nll-0qb4#PSY1z~aJdBK^zqjy3qp4gt#!vvXqyinBED z*nwy4Bh-7)9p*lNXKCIk*Lv7LHSlR~ru|t1%{w;5Sw-CQ8mtWc9(TW5UY=G|?1MrL zL6b%&C`yGjX9tSwrT6q@aNUYnU76$qf7U?rP8qmvMJyjX4d&2(k9$}x3xsP?#Adn; zKhEm)-&uP9-=Vk$zE(VpwSH%5elRZ{R~ugr{spVwS(+a#i?h6QMkSWderI)RStYYP zT9>d5uUqha_!pMgf#RO&S=(JK?f;NBFn?vuX``P^S8f&jM81(13xxeKd{;&$>Uw_wdl%88_7Qe>Y>Xyy_ z%$`@v+=sFItZajG%lx`=0IS#8f#TQV0{vHg1QfbkwnT>LuUqvi8oY-;d&)^`5R-lykQybf-3exMr|@>|>o zztyX}MfKr-_UsY!jJ1bsgK)gJ70iFK1I6_|RLfk8vq7o;jsILeAJh8{#mkYl@Sex- zai9NIR|*ck!k@3(OSP<4@iNEz_jCXAxxJ>>;$LOMivHgFkL~PRwanXl3e!e6^xNpUu_pnvwC>z()XITLva zI^HvFCI?Iom>e)UU~<6ZfXM-q111OldpUr=#rW@~$)v{QfXM-q111Md4wxJ;Ibd?Y ze)UU~<6ZfXM-q111Md4wxJ;Ibd?Y rs@ literal 0 HcmV?d00001 diff --git a/nebula_tests/data/continental/ars548/1708578204/metadata.yaml b/nebula_tests/data/continental/ars548/1708578204/metadata.yaml new file mode 100644 index 000000000..b0905061e --- /dev/null +++ b/nebula_tests/data/continental/ars548/1708578204/metadata.yaml @@ -0,0 +1,26 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 37656875 + starting_time: + nanoseconds_since_epoch: 1709877048960694335 + message_count: 2 + topics_with_message_count: + - topic_metadata: + name: /sensing/radar/front_center/nebula_packets + type: nebula_msgs/msg/NebulaPackets + serialization_format: cdr + offered_qos_profiles: "- history: 1\n depth: 5\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false\n- history: 1\n depth: 10\n reliability: 2\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 2 + compression_format: "" + compression_mode: "" + relative_file_paths: + - 1708578204.db3 + files: + - path: 1708578204.db3 + starting_time: + nanoseconds_since_epoch: 1709877048960694335 + duration: + nanoseconds: 37656875 + message_count: 2 diff --git a/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml b/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml new file mode 100644 index 000000000..d64b06d91 --- /dev/null +++ b/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml @@ -0,0 +1,3257 @@ +header: + stamp: + sec: 1708578204 + nanosec: 199326753 + frame_id: some_sensor_frame +stamp_sync_status: 1 +objects: + - object_id: 21326 + age: 4903 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 10.1338 + y: 4.85423 + z: 0.575499 + position_std: + x: 0.0893600 + y: 0.0612767 + z: 0.0822394 + position_covariance_xy: 0.000433934 + orientation: -0.0263776 + orientation_std: 0.231449 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.281253 + y: 0.0498736 + z: 0.00000 + absolute_velocity_std: + x: 0.0712079 + y: 0.0653650 + z: 0.00000 + absolute_velocity_covariance_xy: 1.39953e-05 + relative_velocity: + x: -0.910200 + y: 0.542119 + z: 0.00000 + relative_velocity_std: + x: 2.50573 + y: 2.51145 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0712079 + y: 0.0653650 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.17124 + shape_width_edge_mean: 1.52149 + - object_id: 15603 + age: 4656 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 12.8366 + y: 4.97638 + z: 0.568930 + position_std: + x: 0.113872 + y: 0.106453 + z: 0.109730 + position_covariance_xy: 0.000683580 + orientation: 0.626398 + orientation_std: 0.347189 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.0249348 + y: -0.182469 + z: 0.00000 + absolute_velocity_std: + x: 0.0657963 + y: 0.0750903 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000650616 + relative_velocity: + x: -0.984014 + y: 0.653441 + z: 0.00000 + relative_velocity_std: + x: 2.51041 + y: 2.52382 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0657963 + y: 0.0750903 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.856446 + shape_width_edge_mean: 1.53718 + - object_id: 713 + age: 3856 + status_measurement: 0 + status_movement: 1 + position_reference: 3 + position: + x: 13.0362 + y: 10.1418 + z: 1.15901 + position_std: + x: 0.0975602 + y: 0.0669582 + z: 0.116364 + position_covariance_xy: -0.00260317 + orientation: -0.0622597 + orientation_std: 0.218113 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.191382 + y: -0.158978 + z: 0.00000 + absolute_velocity_std: + x: 0.0664440 + y: 0.0635578 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00138304 + relative_velocity: + x: -1.11195 + y: 0.597321 + z: 0.00000 + relative_velocity_std: + x: 2.52709 + y: 2.51774 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0664440 + y: 0.0635578 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 4.79050 + shape_width_edge_mean: 3.48760 + - object_id: 6679 + age: 1200 + status_measurement: 0 + status_movement: 1 + position_reference: 3 + position: + x: 17.5246 + y: 13.7744 + z: 1.37556 + position_std: + x: 0.124017 + y: 0.189966 + z: 0.268218 + position_covariance_xy: -0.0143436 + orientation: -1.47839 + orientation_std: 0.457635 + existence_probability: 61.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 80 + classification_unknown: 20 + absolute_velocity: + x: -0.214727 + y: 0.0865279 + z: 0.00000 + absolute_velocity_std: + x: 0.126111 + y: 0.152836 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0139042 + relative_velocity: + x: -1.17718 + y: 0.754112 + z: 0.00000 + relative_velocity_std: + x: 2.54055 + y: 2.53973 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.126111 + y: 0.152836 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.302576 + shape_width_edge_mean: 0.265067 + - object_id: 5190 + age: 2100 + status_measurement: 2 + status_movement: 1 + position_reference: 5 + position: + x: 24.6950 + y: 17.8001 + z: 2.17280 + position_std: + x: 0.103989 + y: 0.0780359 + z: 0.231900 + position_covariance_xy: -0.00199605 + orientation: 0.0768902 + orientation_std: 0.522596 + existence_probability: 63.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 58 + classification_unknown: 42 + absolute_velocity: + x: -0.0428709 + y: 0.114782 + z: 0.00000 + absolute_velocity_std: + x: 0.0775564 + y: 0.0830770 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00292427 + relative_velocity: + x: -1.29056 + y: 0.960184 + z: 0.00000 + relative_velocity_std: + x: 2.56459 + y: 2.59350 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0775564 + y: 0.0830770 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.919289 + shape_width_edge_mean: 0.447692 + - object_id: 13504 + age: 3956 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 25.9748 + y: 14.2498 + z: 1.70027 + position_std: + x: 0.110165 + y: 0.0903350 + z: 0.183149 + position_covariance_xy: -0.00392054 + orientation: -0.168424 + orientation_std: 1.12442 + existence_probability: 99.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 90 + classification_unknown: 10 + absolute_velocity: + x: 0.0397587 + y: 0.0506541 + z: 0.00000 + absolute_velocity_std: + x: 0.0632892 + y: 0.0748134 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00154958 + relative_velocity: + x: -1.18658 + y: 1.02224 + z: 0.00000 + relative_velocity_std: + x: 2.54031 + y: 2.61328 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0632892 + y: 0.0748134 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.47229 + shape_width_edge_mean: 0.656971 + - object_id: 1602 + age: 1903 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 30.5224 + y: 8.02269 + z: 0.419841 + position_std: + x: 0.0928148 + y: 0.0438589 + z: 0.136950 + position_covariance_xy: 0.000466252 + orientation: 0.112420 + orientation_std: 0.303887 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.127808 + y: 0.118150 + z: 0.00000 + absolute_velocity_std: + x: 0.0578545 + y: 0.0602453 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000336696 + relative_velocity: + x: -1.03027 + y: 1.20703 + z: 0.00000 + relative_velocity_std: + x: 2.51552 + y: 2.67493 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0578545 + y: 0.0602453 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 4.68386 + shape_width_edge_mean: 0.629054 + - object_id: 3392 + age: 3556 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 32.7531 + y: 14.6801 + z: 1.98698 + position_std: + x: 0.0919939 + y: 0.0833864 + z: 0.120253 + position_covariance_xy: -2.57981e-05 + orientation: 0.354399 + orientation_std: 0.284635 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.0290804 + y: 0.177214 + z: 0.00000 + absolute_velocity_std: + x: 0.0584801 + y: 0.0691041 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000912535 + relative_velocity: + x: -1.23063 + y: 1.21365 + z: 0.00000 + relative_velocity_std: + x: 2.54977 + y: 2.68214 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0584801 + y: 0.0691041 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 3.57478 + shape_width_edge_mean: 2.60943 + - object_id: 10543 + age: 802 + status_measurement: 2 + status_movement: 1 + position_reference: 3 + position: + x: 33.6191 + y: 25.1946 + z: 2.18566 + position_std: + x: 0.123010 + y: 0.139385 + z: 0.387020 + position_covariance_xy: -0.0109394 + orientation: -0.288580 + orientation_std: 0.244836 + existence_probability: 50.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 58 + classification_unknown: 42 + absolute_velocity: + x: -0.203389 + y: 0.279047 + z: 0.00000 + absolute_velocity_std: + x: 0.183042 + y: 0.229051 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0354256 + relative_velocity: + x: -1.49066 + y: 1.20253 + z: 0.00000 + relative_velocity_std: + x: 2.62402 + y: 2.67671 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.183042 + y: 0.229051 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.249033 + shape_width_edge_mean: 0.254110 + - object_id: 4889 + age: 1956 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 41.2637 + y: 8.67090 + z: 0.104547 + position_std: + x: 0.120995 + y: 0.0861237 + z: 0.131012 + position_covariance_xy: -0.000801501 + orientation: 0.0808711 + orientation_std: 3.14159 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.0162740 + y: -0.00677066 + z: 0.00000 + absolute_velocity_std: + x: 0.0625659 + y: 0.116143 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00246267 + relative_velocity: + x: -1.05351 + y: 1.43864 + z: 0.00000 + relative_velocity_std: + x: 2.51661 + y: 2.78064 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0625659 + y: 0.116143 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.964023 + shape_width_edge_mean: 0.173658 + - object_id: 7872 + age: 7756 + status_measurement: 2 + status_movement: 1 + position_reference: 5 + position: + x: 25.4206 + y: -10.4950 + z: 3.98136 + position_std: + x: 0.0545571 + y: 0.0620298 + z: 0.182855 + position_covariance_xy: 7.21655e-05 + orientation: -0.481675 + orientation_std: 1.19911 + existence_probability: 94.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 22 + absolute_velocity: + x: 0.0438246 + y: -0.0277920 + z: 0.00000 + absolute_velocity_std: + x: 0.0556747 + y: 0.0650753 + z: 0.00000 + absolute_velocity_covariance_xy: 0.000488297 + relative_velocity: + x: -0.486719 + y: 0.985087 + z: 0.00000 + relative_velocity_std: + x: 2.52453 + y: 2.59580 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0556747 + y: 0.0650753 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.733975 + shape_width_edge_mean: 0.499983 + - object_id: 10568 + age: 1556 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 26.5647 + y: 14.5820 + z: 4.33175 + position_std: + x: 0.103251 + y: 0.112736 + z: 0.345418 + position_covariance_xy: -0.00390058 + orientation: 0.645019 + orientation_std: 0.136516 + existence_probability: 99.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.231797 + y: -0.523258 + z: 0.00000 + absolute_velocity_std: + x: 0.0977471 + y: 0.132523 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00814168 + relative_velocity: + x: -1.25380 + y: 0.990140 + z: 0.00000 + relative_velocity_std: + x: 2.54734 + y: 2.60682 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0977471 + y: 0.132523 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.273938 + shape_width_edge_mean: 0.264944 + - object_id: 1001 + age: 13701 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 31.4005 + y: -20.5260 + z: 2.47993 + position_std: + x: 0.101755 + y: 0.0973370 + z: 0.169012 + position_covariance_xy: 0.00576165 + orientation: 0.511709 + orientation_std: 1.00065 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 70 + classification_unknown: 0 + absolute_velocity: + x: 0.0535885 + y: 0.0459832 + z: 0.00000 + absolute_velocity_std: + x: 0.0645606 + y: 0.0723749 + z: 0.00000 + absolute_velocity_covariance_xy: 0.00183951 + relative_velocity: + x: -0.226133 + y: 1.18608 + z: 0.00000 + relative_velocity_std: + x: 2.58073 + y: 2.66212 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0645606 + y: 0.0723749 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 3.80737 + shape_width_edge_mean: 0.764181 + - object_id: 9946 + age: 4156 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 41.2143 + y: 13.9063 + z: 2.48051 + position_std: + x: 0.0794138 + y: 0.147930 + z: 0.196328 + position_covariance_xy: -0.00428435 + orientation: 1.44031 + orientation_std: 0.411446 + existence_probability: 93.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 75 + classification_unknown: 0 + absolute_velocity: + x: 0.0554839 + y: -0.121630 + z: 0.00000 + absolute_velocity_std: + x: 0.0586961 + y: 0.0837489 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00148773 + relative_velocity: + x: -1.26971 + y: 1.40356 + z: 0.00000 + relative_velocity_std: + x: 2.55329 + y: 2.76855 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0586961 + y: 0.0837489 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.98885 + shape_width_edge_mean: 1.27152 + - object_id: 16837 + age: 4800 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 42.7820 + y: 6.42565 + z: 0.608283 + position_std: + x: 0.0759149 + y: 0.0696691 + z: 0.146871 + position_covariance_xy: 0.00135767 + orientation: 0.369702 + orientation_std: 0.172958 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.291934 + y: -0.201305 + z: 0.00000 + absolute_velocity_std: + x: 0.0540363 + y: 0.0664084 + z: 0.00000 + absolute_velocity_covariance_xy: -9.74873e-05 + relative_velocity: + x: -1.00972 + y: 1.45427 + z: 0.00000 + relative_velocity_std: + x: 2.51047 + y: 2.79635 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0540363 + y: 0.0664084 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.06955 + shape_width_edge_mean: 1.11864 + - object_id: 18302 + age: 4500 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 42.9615 + y: 13.1683 + z: 1.31383 + position_std: + x: 0.0851742 + y: 0.0347946 + z: 0.147702 + position_covariance_xy: 0.000803070 + orientation: 0.131058 + orientation_std: 2.14588 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.0219594 + y: 0.0224578 + z: 0.00000 + absolute_velocity_std: + x: 0.0550997 + y: 0.0531875 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000143513 + relative_velocity: + x: -1.18873 + y: 1.54025 + z: 0.00000 + relative_velocity_std: + x: 2.53747 + y: 2.83534 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0550997 + y: 0.0531875 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 7.31045 + shape_width_edge_mean: 0.999114 + - object_id: 1332 + age: 656 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 43.0822 + y: 20.0110 + z: 3.10628 + position_std: + x: 0.0688106 + y: 0.0824286 + z: 0.365878 + position_covariance_xy: -0.00118077 + orientation: 0.447546 + orientation_std: 2.12686 + existence_probability: 69.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 8 + classification_unknown: 0 + absolute_velocity: + x: -0.0848573 + y: 0.137693 + z: 0.00000 + absolute_velocity_std: + x: 0.126159 + y: 0.226389 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0220796 + relative_velocity: + x: -1.37144 + y: 1.47378 + z: 0.00000 + relative_velocity_std: + x: 2.58221 + y: 2.80075 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.126159 + y: 0.226389 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.837111 + shape_width_edge_mean: 0.467396 + - object_id: 9615 + age: 856 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 47.2761 + y: 22.4866 + z: 3.79678 + position_std: + x: 0.0776102 + y: 0.100969 + z: 0.365603 + position_covariance_xy: -0.00349093 + orientation: 0.140662 + orientation_std: 0.0941195 + existence_probability: 69.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.345125 + y: 0.752020 + z: 0.00000 + absolute_velocity_std: + x: 0.121289 + y: 0.219734 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0213379 + relative_velocity: + x: -1.39940 + y: 1.60345 + z: 0.00000 + relative_velocity_std: + x: 2.59979 + y: 2.86174 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.121289 + y: 0.219734 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.499058 + shape_width_edge_mean: 0.221308 + - object_id: 6057 + age: 1603 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 55.7696 + y: 13.8419 + z: 1.36511 + position_std: + x: 0.0839208 + y: 0.0379766 + z: 0.177101 + position_covariance_xy: 0.000158241 + orientation: 0.0885069 + orientation_std: 0.224375 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.112390 + y: 0.228768 + z: 0.00000 + absolute_velocity_std: + x: 0.0579406 + y: 0.0627878 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000490738 + relative_velocity: + x: -1.20415 + y: 1.92333 + z: 0.00000 + relative_velocity_std: + x: 2.54062 + y: 3.05601 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0579406 + y: 0.0627878 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 5.93226 + shape_width_edge_mean: 0.613519 + - object_id: 17101 + age: 703 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 58.2548 + y: 7.49128 + z: 0.908498 + position_std: + x: 0.0725007 + y: 0.201025 + z: 0.372218 + position_covariance_xy: -0.00499407 + orientation: 0.236209 + orientation_std: 1.08127 + existence_probability: 69.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.0201362 + y: 0.101298 + z: 0.00000 + absolute_velocity_std: + x: 0.0921021 + y: 0.422812 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0235889 + relative_velocity: + x: -1.02556 + y: 1.89466 + z: 0.00000 + relative_velocity_std: + x: 2.51273 + y: 3.03767 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0921021 + y: 0.422812 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.245004 + shape_width_edge_mean: 0.245003 + - object_id: 328 + age: 256 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 63.9131 + y: -25.1119 + z: 3.53112 + position_std: + x: 0.109699 + y: 0.173494 + z: 1.05312 + position_covariance_xy: 0.00579861 + orientation: -0.913433 + orientation_std: 1.37511 + existence_probability: 69.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 20 + absolute_velocity: + x: -0.158842 + y: 0.129240 + z: 0.00000 + absolute_velocity_std: + x: 0.194185 + y: 0.442800 + z: 0.00000 + absolute_velocity_covariance_xy: 0.0697472 + relative_velocity: + x: -0.103980 + y: 2.08868 + z: 0.00000 + relative_velocity_std: + x: 2.62523 + y: 3.14233 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.194185 + y: 0.442800 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.321030 + shape_width_edge_mean: 0.327726 + - object_id: 15340 + age: 2856 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 64.6681 + y: 14.2275 + z: 0.641320 + position_std: + x: 0.0730478 + y: 0.0852915 + z: 0.277308 + position_covariance_xy: -0.00135582 + orientation: 0.0478349 + orientation_std: 0.251724 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.0943105 + y: 0.193419 + z: 0.00000 + absolute_velocity_std: + x: 0.0568863 + y: 0.0836396 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00114578 + relative_velocity: + x: -1.21806 + y: 2.09206 + z: 0.00000 + relative_velocity_std: + x: 2.54198 + y: 3.16913 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0568863 + y: 0.0836396 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.645617 + shape_width_edge_mean: 0.275392 + - object_id: 13507 + age: 556 + status_measurement: 2 + status_movement: 1 + position_reference: 5 + position: + x: 75.9817 + y: 23.7325 + z: 3.04780 + position_std: + x: 0.121282 + y: 0.265907 + z: 1.41356 + position_covariance_xy: -0.0195009 + orientation: 0.00000 + orientation_std: 0.498183 + existence_probability: 74.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 8 + classification_unknown: 38 + absolute_velocity: + x: -0.0453051 + y: 0.185729 + z: 0.00000 + absolute_velocity_std: + x: 0.176155 + y: 0.503230 + z: 0.00000 + absolute_velocity_covariance_xy: -0.0765525 + relative_velocity: + x: -1.48740 + y: 2.38361 + z: 0.00000 + relative_velocity_std: + x: 2.60972 + y: 3.38718 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.176155 + y: 0.503230 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.242340 + shape_width_edge_mean: 0.242338 + - object_id: 23112 + age: 2001 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 81.8113 + y: 17.4929 + z: 1.90293 + position_std: + x: 0.109619 + y: 0.0616260 + z: 0.238849 + position_covariance_xy: 0.000133373 + orientation: 0.161192 + orientation_std: 0.134398 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.127906 + y: 0.389968 + z: 0.00000 + absolute_velocity_std: + x: 0.0569762 + y: 0.0727686 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000590431 + relative_velocity: + x: -1.32603 + y: 2.64514 + z: 0.00000 + relative_velocity_std: + x: 2.56506 + y: 3.58350 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0569762 + y: 0.0727686 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 5.36073 + shape_width_edge_mean: 0.755033 + - object_id: 8776 + age: 201 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 87.7591 + y: -16.4986 + z: 1.78075 + position_std: + x: 0.137807 + y: 0.373512 + z: 0.874648 + position_covariance_xy: 0.0183878 + orientation: -0.0104023 + orientation_std: 3.14159 + existence_probability: 96.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 60 + classification_unknown: 40 + absolute_velocity: + x: 0.125606 + y: -0.0622319 + z: 0.00000 + absolute_velocity_std: + x: 0.174239 + y: 0.721147 + z: 0.00000 + absolute_velocity_covariance_xy: 0.0813062 + relative_velocity: + x: -0.376705 + y: 2.72773 + z: 0.00000 + relative_velocity_std: + x: 2.55653 + y: 3.63894 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.174239 + y: 0.721147 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.238447 + shape_width_edge_mean: 0.238443 + - object_id: 15044 + age: 5356 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 90.5687 + y: 5.36355 + z: 3.71010 + position_std: + x: 0.0904763 + y: 0.176544 + z: 0.323580 + position_covariance_xy: -0.00468330 + orientation: -1.10519 + orientation_std: 0.136743 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.0450059 + y: -0.377829 + z: 0.00000 + absolute_velocity_std: + x: 0.0548296 + y: 0.0911363 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000487374 + relative_velocity: + x: -0.954764 + y: 2.83381 + z: 0.00000 + relative_velocity_std: + x: 2.50331 + y: 3.73564 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0548296 + y: 0.0911363 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.65078 + shape_width_edge_mean: 0.624680 + - object_id: 17741 + age: 4003 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 92.0528 + y: -4.42428 + z: 4.34049 + position_std: + x: 0.100531 + y: 0.195942 + z: 0.392340 + position_covariance_xy: 0.0118836 + orientation: 0.969461 + orientation_std: 0.575154 + existence_probability: 99.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 16 + absolute_velocity: + x: 0.0975171 + y: 0.0818781 + z: 0.00000 + absolute_velocity_std: + x: 0.0561936 + y: 0.0973063 + z: 0.00000 + absolute_velocity_covariance_xy: 0.000776284 + relative_velocity: + x: -0.705039 + y: 2.83373 + z: 0.00000 + relative_velocity_std: + x: 2.50615 + y: 3.73267 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0561936 + y: 0.0973063 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.02040 + shape_width_edge_mean: 0.443903 + - object_id: 12038 + age: 4256 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 94.5779 + y: 11.2813 + z: 3.29155 + position_std: + x: 0.0853438 + y: 0.103357 + z: 0.242284 + position_covariance_xy: 0.00124946 + orientation: 0.220231 + orientation_std: 0.324586 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.134249 + y: -0.133708 + z: 0.00000 + absolute_velocity_std: + x: 0.0550460 + y: 0.0781126 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000362847 + relative_velocity: + x: -1.17226 + y: 2.94003 + z: 0.00000 + relative_velocity_std: + x: 2.52604 + y: 3.83480 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0550460 + y: 0.0781126 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 4.26629 + shape_width_edge_mean: 2.30805 + - object_id: 7875 + age: 3300 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 98.4294 + y: 21.7438 + z: 4.00860 + position_std: + x: 0.0815024 + y: 0.0924730 + z: 0.306751 + position_covariance_xy: -0.00114342 + orientation: 0.150714 + orientation_std: 0.0666051 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.164830 + y: 0.760716 + z: 0.00000 + absolute_velocity_std: + x: 0.0575779 + y: 0.0859836 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00123049 + relative_velocity: + x: -1.43933 + y: 3.06211 + z: 0.00000 + relative_velocity_std: + x: 2.59889 + y: 3.93241 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0575779 + y: 0.0859836 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.91700 + shape_width_edge_mean: 0.776736 + - object_id: 23420 + age: 4102 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 99.1534 + y: -4.58107 + z: 4.44346 + position_std: + x: 0.0893315 + y: 0.112605 + z: 0.432576 + position_covariance_xy: 0.00252389 + orientation: 0.276770 + orientation_std: 0.310382 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.0129893 + y: -0.167267 + z: 0.00000 + absolute_velocity_std: + x: 0.0557474 + y: 0.0802356 + z: 0.00000 + absolute_velocity_covariance_xy: 0.000267921 + relative_velocity: + x: -0.726504 + y: 3.07596 + z: 0.00000 + relative_velocity_std: + x: 2.50620 + y: 3.93251 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0557474 + y: 0.0802356 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.21515 + shape_width_edge_mean: 0.450654 + - object_id: 3914 + age: 2700 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 100.600 + y: 4.01965 + z: 1.20270 + position_std: + x: 0.132183 + y: 0.213027 + z: 0.314639 + position_covariance_xy: -0.00224913 + orientation: 0.0774307 + orientation_std: 0.290845 + existence_probability: 98.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.000804091 + y: 0.203290 + z: 0.00000 + absolute_velocity_std: + x: 0.0607859 + y: 0.140619 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00103899 + relative_velocity: + x: -0.956246 + y: 3.07861 + z: 0.00000 + relative_velocity_std: + x: 2.50477 + y: 3.94185 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0607859 + y: 0.140619 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.396758 + shape_width_edge_mean: 0.255384 + - object_id: 6062 + age: 3201 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 103.670 + y: 21.9804 + z: 2.04965 + position_std: + x: 0.0951003 + y: 0.116310 + z: 0.266681 + position_covariance_xy: -0.00340519 + orientation: -0.275362 + orientation_std: 0.0857875 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.107615 + y: 0.601012 + z: 0.00000 + absolute_velocity_std: + x: 0.0564564 + y: 0.0810774 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000860845 + relative_velocity: + x: -1.43592 + y: 3.20379 + z: 0.00000 + relative_velocity_std: + x: 2.59380 + y: 4.05849 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0564564 + y: 0.0810774 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.75171 + shape_width_edge_mean: 2.43531 + - object_id: 3398 + age: 1556 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 104.943 + y: 10.8464 + z: 1.25853 + position_std: + x: 0.0385328 + y: 0.135561 + z: 0.331425 + position_covariance_xy: -0.00205166 + orientation: 0.248431 + orientation_std: 0.0851209 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -2.25405 + y: -1.03053 + z: 0.00000 + absolute_velocity_std: + x: 0.0586001 + y: 0.188223 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00395837 + relative_velocity: + x: -1.22714 + y: 3.34057 + z: 0.00000 + relative_velocity_std: + x: 2.52537 + y: 4.06161 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0586001 + y: 0.188223 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.223868 + shape_width_edge_mean: 0.258396 + - object_id: 15922 + age: 403 + status_measurement: 2 + status_movement: 1 + position_reference: 5 + position: + x: 105.022 + y: -11.9400 + z: 3.21213 + position_std: + x: 0.134557 + y: 0.420694 + z: 2.19951 + position_covariance_xy: 0.0126895 + orientation: 0.0410038 + orientation_std: 3.14159 + existence_probability: 34.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 2 + classification_unknown: 48 + absolute_velocity: + x: 0.0109977 + y: -0.0652869 + z: 0.00000 + absolute_velocity_std: + x: 0.130954 + y: 0.833482 + z: 0.00000 + absolute_velocity_covariance_xy: 0.0624928 + relative_velocity: + x: -0.527422 + y: 3.22378 + z: 0.00000 + relative_velocity_std: + x: 2.52987 + y: 4.05763 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.130954 + y: 0.833482 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.313649 + shape_width_edge_mean: 0.238129 + - object_id: 5788 + age: 3656 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 112.569 + y: -3.88744 + z: 4.00191 + position_std: + x: 0.0820184 + y: 0.127916 + z: 0.504295 + position_covariance_xy: 0.00293168 + orientation: 0.347114 + orientation_std: 0.381038 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.00949418 + y: -0.133085 + z: 0.00000 + absolute_velocity_std: + x: 0.0543883 + y: 0.0882754 + z: 0.00000 + absolute_velocity_covariance_xy: 0.000239605 + relative_velocity: + x: -0.760108 + y: 3.43904 + z: 0.00000 + relative_velocity_std: + x: 2.50475 + y: 4.25567 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0543883 + y: 0.0882754 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.00134 + shape_width_edge_mean: 0.568087 + - object_id: 7216 + age: 3356 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 113.889 + y: 23.2102 + z: 3.51649 + position_std: + x: 0.100234 + y: 0.118002 + z: 0.326909 + position_covariance_xy: -0.00273509 + orientation: -0.00746778 + orientation_std: 0.493474 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.0153468 + y: 0.131596 + z: 0.00000 + absolute_velocity_std: + x: 0.0575961 + y: 0.0822244 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000912169 + relative_velocity: + x: -1.50830 + y: 3.51450 + z: 0.00000 + relative_velocity_std: + x: 2.60466 + y: 4.34344 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0575961 + y: 0.0822244 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 3.48359 + shape_width_edge_mean: 0.611648 + - object_id: 14710 + age: 3502 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 114.427 + y: 13.6533 + z: 1.79124 + position_std: + x: 0.0780441 + y: 0.115176 + z: 0.448395 + position_covariance_xy: -3.12266e-05 + orientation: 0.290389 + orientation_std: 0.381254 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.0718545 + y: -0.136950 + z: 0.00000 + absolute_velocity_std: + x: 0.0561767 + y: 0.0942400 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000855894 + relative_velocity: + x: -1.26041 + y: 3.47640 + z: 0.00000 + relative_velocity_std: + x: 2.53861 + y: 4.30621 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0561767 + y: 0.0942400 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.49465 + shape_width_edge_mean: 0.726293 + - object_id: 1607 + age: 1656 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 118.710 + y: -3.12502 + z: 3.68525 + position_std: + x: 0.0984055 + y: 0.259171 + z: 0.899835 + position_covariance_xy: 0.00144230 + orientation: 0.0365639 + orientation_std: 0.135611 + existence_probability: 98.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 10 + absolute_velocity: + x: 0.0360527 + y: 0.403528 + z: 0.00000 + absolute_velocity_std: + x: 0.0628415 + y: 0.288742 + z: 0.00000 + absolute_velocity_covariance_xy: 0.000296791 + relative_velocity: + x: -0.775318 + y: 3.61217 + z: 0.00000 + relative_velocity_std: + x: 2.50296 + y: 4.41640 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0628415 + y: 0.288742 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.605599 + shape_width_edge_mean: 0.469470 + - object_id: 429 + age: 2503 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 119.957 + y: 14.2069 + z: 2.24944 + position_std: + x: 0.0966874 + y: 0.0742177 + z: 0.285030 + position_covariance_xy: -6.78698e-05 + orientation: 0.0765282 + orientation_std: 0.113911 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 93 + classification_unknown: 0 + absolute_velocity: + x: -0.0201783 + y: 0.466035 + z: 0.00000 + absolute_velocity_std: + x: 0.0561868 + y: 0.0879245 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000739614 + relative_velocity: + x: -1.25923 + y: 3.70304 + z: 0.00000 + relative_velocity_std: + x: 2.54404 + y: 4.51083 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0561868 + y: 0.0879245 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 5.46425 + shape_width_edge_mean: 0.895933 + - object_id: 430 + age: 2503 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 120.119 + y: 23.6486 + z: 5.72320 + position_std: + x: 0.117110 + y: 0.0930383 + z: 0.399529 + position_covariance_xy: -0.000721470 + orientation: 0.0618971 + orientation_std: 0.515015 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.0235011 + y: 0.129880 + z: 0.00000 + absolute_velocity_std: + x: 0.0584178 + y: 0.0770805 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000811883 + relative_velocity: + x: -1.53408 + y: 3.71531 + z: 0.00000 + relative_velocity_std: + x: 2.61089 + y: 4.53165 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0584178 + y: 0.0770805 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 5.11301 + shape_width_edge_mean: 0.819783 + - object_id: 728 + age: 756 + status_measurement: 0 + status_movement: 1 + position_reference: 7 + position: + x: 121.901 + y: -63.5762 + z: 23.0433 + position_std: + x: 0.249524 + y: 0.312451 + z: 0.644216 + position_covariance_xy: 0.0520361 + orientation: 0.927265 + orientation_std: 0.0514514 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.792093 + y: -1.67203 + z: 0.00000 + absolute_velocity_std: + x: 0.270252 + y: 0.530445 + z: 0.00000 + absolute_velocity_covariance_xy: 0.135914 + relative_velocity: + x: 0.809108 + y: 3.81977 + z: 0.00000 + relative_velocity_std: + x: 3.19778 + y: 4.51818 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.270252 + y: 0.530445 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.22980 + shape_width_edge_mean: 1.16521 + - object_id: 26411 + age: 456 + status_measurement: 2 + status_movement: 1 + position_reference: 7 + position: + x: 124.522 + y: -63.5269 + z: 4.84977 + position_std: + x: 0.183149 + y: 0.298703 + z: 1.19414 + position_covariance_xy: 0.0447042 + orientation: 0.575237 + orientation_std: 0.903935 + existence_probability: 34.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 48 + absolute_velocity: + x: -0.0477202 + y: -0.307551 + z: 0.00000 + absolute_velocity_std: + x: 0.362267 + y: 0.718918 + z: 0.00000 + absolute_velocity_covariance_xy: 0.246964 + relative_velocity: + x: 0.901648 + y: 3.86728 + z: 0.00000 + relative_velocity_std: + x: 3.20887 + y: 4.60244 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.362267 + y: 0.718918 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 2.05817 + shape_width_edge_mean: 1.28138 + - object_id: 27905 + age: 2400 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 128.066 + y: 15.0184 + z: 1.54205 + position_std: + x: 0.0909826 + y: 0.121686 + z: 0.350600 + position_covariance_xy: 0.000221836 + orientation: 0.158546 + orientation_std: 0.197063 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: -0.0179684 + y: 0.271318 + z: 0.00000 + absolute_velocity_std: + x: 0.0576106 + y: 0.133280 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00190523 + relative_velocity: + x: -1.29726 + y: 3.89322 + z: 0.00000 + relative_velocity_std: + x: 2.54846 + y: 4.69011 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0576106 + y: 0.133280 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 3.23510 + shape_width_edge_mean: 1.21448 + - object_id: 1004 + age: 2956 + status_measurement: 0 + status_movement: 1 + position_reference: 7 + position: + x: 128.285 + y: -54.4384 + z: 25.6694 + position_std: + x: 0.0937111 + y: 0.227495 + z: 0.331697 + position_covariance_xy: 0.0147662 + orientation: 1.28680 + orientation_std: 0.326283 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.0793685 + y: 0.140228 + z: 0.00000 + absolute_velocity_std: + x: 0.0724348 + y: 0.142356 + z: 0.00000 + absolute_velocity_covariance_xy: 0.00627484 + relative_velocity: + x: 0.634910 + y: 3.94719 + z: 0.00000 + relative_velocity_std: + x: 3.01610 + y: 4.69261 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0724348 + y: 0.142356 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.14003 + shape_width_edge_mean: 0.409460 + - object_id: 10556 + age: 3956 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 129.434 + y: 24.1480 + z: 3.29694 + position_std: + x: 0.0851414 + y: 0.0782793 + z: 0.259771 + position_covariance_xy: 4.15599e-05 + orientation: 0.295748 + orientation_std: 0.230501 + existence_probability: 97.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: 0.124796 + y: 0.235003 + z: 0.00000 + absolute_velocity_std: + x: 0.0553814 + y: 0.0697167 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000379358 + relative_velocity: + x: -1.54318 + y: 3.89800 + z: 0.00000 + relative_velocity_std: + x: 2.61402 + y: 4.70932 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0553814 + y: 0.0697167 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 4.66514 + shape_width_edge_mean: 2.76232 + - object_id: 5482 + age: 201 + status_measurement: 0 + status_movement: 1 + position_reference: 3 + position: + x: 130.531 + y: 45.7412 + z: 6.05400 + position_std: + x: 0.249557 + y: 0.499463 + z: 3.98007 + position_covariance_xy: -0.0712331 + orientation: -0.901585 + orientation_std: 3.00885 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 40 + absolute_velocity: + x: -0.0245859 + y: 0.0641377 + z: 0.00000 + absolute_velocity_std: + x: 0.322114 + y: 0.885638 + z: 0.00000 + absolute_velocity_covariance_xy: -0.261546 + relative_velocity: + x: -2.16580 + y: 3.90650 + z: 0.00000 + relative_velocity_std: + x: 2.88850 + y: 4.72644 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.322114 + y: 0.885638 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.242643 + shape_width_edge_mean: 0.242643 + - object_id: 12952 + age: 3256 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 134.431 + y: 15.0199 + z: 1.41306 + position_std: + x: 0.104318 + y: 0.159561 + z: 0.324778 + position_covariance_xy: -0.00128720 + orientation: 0.187701 + orientation_std: 0.167732 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 100 + classification_unknown: 0 + absolute_velocity: + x: 0.0428570 + y: -0.312525 + z: 0.00000 + absolute_velocity_std: + x: 0.0565324 + y: 0.106847 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00109494 + relative_velocity: + x: -1.31756 + y: 4.05010 + z: 0.00000 + relative_velocity_std: + x: 2.54464 + y: 4.84238 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0565324 + y: 0.106847 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.03018 + shape_width_edge_mean: 0.409184 + - object_id: 1309 + age: 900 + status_measurement: 2 + status_movement: 1 + position_reference: 7 + position: + x: 135.359 + y: -78.2726 + z: 4.23004 + position_std: + x: 0.288218 + y: 0.489971 + z: 1.32659 + position_covariance_xy: 0.132308 + orientation: 1.00738 + orientation_std: 0.849589 + existence_probability: 31.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 48 + absolute_velocity: + x: 0.791450 + y: -0.00779335 + z: 0.00000 + absolute_velocity_std: + x: 0.418619 + y: 0.730855 + z: 0.00000 + absolute_velocity_covariance_xy: 0.297052 + relative_velocity: + x: 1.34383 + y: 4.08997 + z: 0.00000 + relative_velocity_std: + x: 3.53215 + y: 4.84785 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.418619 + y: 0.730855 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 0.499513 + shape_width_edge_mean: 0.229559 + - object_id: 1008 + age: 2902 + status_measurement: 2 + status_movement: 1 + position_reference: 5 + position: + x: 138.501 + y: 24.2786 + z: 4.51527 + position_std: + x: 0.119441 + y: 0.135968 + z: 0.511622 + position_covariance_xy: -0.00245312 + orientation: 0.106663 + orientation_std: 0.381638 + existence_probability: 35.0000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 26 + absolute_velocity: + x: 0.124690 + y: 0.174770 + z: 0.00000 + absolute_velocity_std: + x: 0.0615881 + y: 0.0976572 + z: 0.00000 + absolute_velocity_covariance_xy: -0.00123114 + relative_velocity: + x: -1.55769 + y: 4.11438 + z: 0.00000 + relative_velocity_std: + x: 2.61555 + y: 4.91628 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0615881 + y: 0.0976572 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 1.79034 + shape_width_edge_mean: 0.368496 + - object_id: 1603 + age: 2956 + status_measurement: 0 + status_movement: 1 + position_reference: 5 + position: + x: 138.908 + y: 15.8486 + z: 3.60274 + position_std: + x: 0.0924078 + y: 0.0617177 + z: 0.300519 + position_covariance_xy: -0.000842813 + orientation: -0.0556955 + orientation_std: 0.445778 + existence_probability: 100.000 + classification_car: 0 + classification_truck: 0 + classification_motorcycle: 0 + classification_bicycle: 0 + classification_pedestrian: 0 + classification_animal: 0 + classification_hazard: 0 + classification_unknown: 0 + absolute_velocity: + x: -0.000240124 + y: 0.126967 + z: 0.00000 + absolute_velocity_std: + x: 0.0550816 + y: 0.0680058 + z: 0.00000 + absolute_velocity_covariance_xy: -0.000351060 + relative_velocity: + x: -1.32072 + y: 4.22811 + z: 0.00000 + relative_velocity_std: + x: 2.54961 + y: 5.01225 + z: 0.00000 + relative_velocity_covariance_xy: 0.00000 + absolute_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + absolute_acceleration_std: + x: 0.0550816 + y: 0.0680058 + z: 0.00000 + absolute_acceleration_covariance_xy: 6.25000 + relative_acceleration: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_std: + x: 0.00000 + y: 0.00000 + z: 0.00000 + relative_acceleration_covariance_xy: 0.00000 + orientation_rate_mean: 0.00000 + orientation_rate_std: 0.0519615 + shape_length_edge_mean: 4.37316 + shape_width_edge_mean: 1.27794 diff --git a/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml b/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml new file mode 100644 index 000000000..6268cc44a --- /dev/null +++ b/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml @@ -0,0 +1,7104 @@ +header: + stamp: + sec: 1708578204 + nanosec: 202447652 + frame_id: some_base_frame +stamp_sync_status: 1 +origin_pos: + x: 3.87200 + y: 0.00000 + z: 0.641000 +origin_pitch: 0.00000 +origin_pitch_std: 0.00493735 +origin_yaw: 0.00000 +origin_yaw_std: 0.0263817 +ambiguity_free_velocity_min: -22.8866 +ambiguity_free_velocity_max: 22.8866 +alignment_azimuth_correction: 0.00000 +alignment_elevation_correction: 0.00000 +alignment_status: 0 +detections: + - azimuth_angle: -0.918583 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.184710 + elevation_angle_std: 0.0731464 + range: 3.37381 + range_std: 0.200000 + range_rate: -3.64937 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 3 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 20 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.905259 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.160397 + elevation_angle_std: 0.0731464 + range: 3.43046 + range_std: 0.200000 + range_rate: -3.68150 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 5 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 9 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.876427 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.292543 + elevation_angle_std: 0.0731464 + range: 3.50288 + range_std: 0.200000 + range_rate: -3.82053 + range_rate_std: 0.250000 + rcs: -4 + measurement_id: 6 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 1 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.806051 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.192620 + elevation_angle_std: 0.0731464 + range: 3.88876 + range_std: 0.200000 + range_rate: -4.19739 + range_rate_std: 0.250000 + rcs: -13 + measurement_id: 12 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 30 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.751149 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.163070 + elevation_angle_std: 0.0731464 + range: 3.98426 + range_std: 0.200000 + range_rate: -4.41912 + range_rate_std: 0.250000 + rcs: -13 + measurement_id: 11 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 10 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.667011 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.207217 + elevation_angle_std: 0.0706906 + range: 4.02782 + range_std: 0.200000 + range_rate: -4.80569 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 8 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 22 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.719778 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.303270 + elevation_angle_std: 0.0731464 + range: 4.24818 + range_std: 0.200000 + range_rate: -4.51565 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 19 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 25 + object_id: 0 + ambiguity_flag: 50 + - azimuth_angle: -0.652473 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.420943 + elevation_angle_std: 0.0731464 + range: 4.59878 + range_std: 0.200000 + range_rate: -4.76009 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 17 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.578515 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.142399 + elevation_angle_std: 0.0731464 + range: 4.77610 + range_std: 0.200000 + range_rate: -5.06903 + range_rate_std: 0.250000 + rcs: -15 + measurement_id: 14 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 0 + object_id: 0 + ambiguity_flag: 70 + - azimuth_angle: -0.560304 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.338562 + elevation_angle_std: 0.0731464 + range: 5.14421 + range_std: 0.200000 + range_rate: -5.11338 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 20 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 7 + object_id: 0 + ambiguity_flag: 70 + - azimuth_angle: -0.564549 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.340141 + elevation_angle_std: 0.0502777 + range: 5.16376 + range_std: 0.200000 + range_rate: -5.09361 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 23 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 10 + object_id: 0 + ambiguity_flag: 70 + - azimuth_angle: -0.529609 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.130830 + elevation_angle_std: 0.0448793 + range: 5.30253 + range_std: 0.200000 + range_rate: -5.25191 + range_rate_std: 0.250000 + rcs: -12 + measurement_id: 21 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 19 + object_id: 15349 + ambiguity_flag: 0 + - azimuth_angle: -0.479294 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.109715 + elevation_angle_std: 0.0631005 + range: 5.54535 + range_std: 0.200000 + range_rate: -5.38589 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 24 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 29 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.453836 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.108035 + elevation_angle_std: 0.0731464 + range: 5.79818 + range_std: 0.200000 + range_rate: -5.44975 + range_rate_std: 0.250000 + rcs: -17 + measurement_id: 25 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 68 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.325422 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.357914 + elevation_angle_std: 0.0731464 + range: 6.62226 + range_std: 0.200000 + range_rate: -5.78322 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 31 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 18 + object_id: 15349 + ambiguity_flag: 0 + - azimuth_angle: -0.378747 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0607697 + elevation_angle_std: 0.0731464 + range: 6.63414 + range_std: 0.200000 + range_rate: -5.68913 + range_rate_std: 0.250000 + rcs: -17 + measurement_id: 30 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 29 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: -0.341595 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0314243 + elevation_angle_std: 0.0631005 + range: 7.41802 + range_std: 0.200000 + range_rate: -5.79949 + range_rate_std: 0.250000 + rcs: -18 + measurement_id: 34 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 27 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.588471 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0196684 + elevation_angle_std: 0.0706906 + range: 7.44410 + range_std: 0.200000 + range_rate: -5.18947 + range_rate_std: 0.250000 + rcs: -4 + measurement_id: 32 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 19 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: -0.304035 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0680587 + elevation_angle_std: 0.0731464 + range: 7.46604 + range_std: 0.200000 + range_rate: -5.83967 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 35 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 4 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.555694 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.188774 + elevation_angle_std: 0.0731464 + range: 7.80573 + range_std: 0.200000 + range_rate: -5.29154 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 38 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 23 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: 0.532446 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.336356 + elevation_angle_std: 0.0563254 + range: 7.97545 + range_std: 0.200000 + range_rate: -5.34590 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 36 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: 0.574514 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.324163 + elevation_angle_std: 0.0731464 + range: 8.21384 + range_std: 0.200000 + range_rate: -5.23066 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 43 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 53 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.598009 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.115062 + elevation_angle_std: 0.0731464 + range: 8.30807 + range_std: 0.200000 + range_rate: -5.12067 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 44 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 48 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: 0.728035 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0238691 + elevation_angle_std: 0.0706906 + range: 8.33191 + range_std: 0.200000 + range_rate: -4.67865 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 41 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: 0.517451 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0201284 + elevation_angle_std: 0.0731464 + range: 8.33446 + range_std: 0.200000 + range_rate: -5.40353 + range_rate_std: 0.250000 + rcs: -18 + measurement_id: 46 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 13 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.625697 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.219443 + elevation_angle_std: 0.0731464 + range: 8.39911 + range_std: 0.200000 + range_rate: -5.03771 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 42 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 19 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: 0.667346 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.129451 + elevation_angle_std: 0.0563254 + range: 8.67152 + range_std: 0.200000 + range_rate: -4.88245 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 40 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 6 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: -0.244306 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.160948 + elevation_angle_std: 0.0731464 + range: 9.01322 + range_std: 0.200000 + range_rate: -5.95376 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 48 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 26 + object_id: 15349 + ambiguity_flag: 0 + - azimuth_angle: 0.486410 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0610122 + elevation_angle_std: 0.0133411 + range: 9.09675 + range_std: 0.200000 + range_rate: -5.48862 + range_rate_std: 0.250000 + rcs: -13 + measurement_id: 47 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 38 + object_id: 21326 + ambiguity_flag: 0 + - azimuth_angle: 0.568922 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0968048 + elevation_angle_std: 0.0448793 + range: 9.19728 + range_std: 0.200000 + range_rate: -5.21861 + range_rate_std: 0.250000 + rcs: -19 + measurement_id: 50 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 9 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.448714 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0512419 + elevation_angle_std: 0.0731464 + range: 9.92080 + range_std: 0.200000 + range_rate: -5.59849 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 54 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 21326 + ambiguity_flag: 100 + - azimuth_angle: -0.234275 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.207909 + elevation_angle_std: 0.0631005 + range: 10.0441 + range_std: 0.200000 + range_rate: -5.92764 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 55 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 9 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.203678 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0444807 + elevation_angle_std: 0.0731464 + range: 10.2911 + range_std: 0.200000 + range_rate: -6.02475 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 56 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.408038 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0744289 + elevation_angle_std: 0.0706906 + range: 10.5996 + range_std: 0.200000 + range_rate: -5.68950 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 58 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 15603 + ambiguity_flag: 100 + - azimuth_angle: 0.565199 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0969142 + elevation_angle_std: 0.0264734 + range: 10.6577 + range_std: 0.200000 + range_rate: -5.23312 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 57 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 11 + object_id: 15603 + ambiguity_flag: 100 + - azimuth_angle: 0.477277 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0836923 + elevation_angle_std: 0.0502777 + range: 10.8006 + range_std: 0.200000 + range_rate: -5.51228 + range_rate_std: 0.250000 + rcs: -18 + measurement_id: 61 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 6 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.227338 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0555324 + elevation_angle_std: 0.0285679 + range: 11.1051 + range_std: 0.200000 + range_rate: -5.96369 + range_rate_std: 0.250000 + rcs: -13 + measurement_id: 59 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 31 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: -0.188587 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0234091 + elevation_angle_std: 0.0502777 + range: 11.3579 + range_std: 0.200000 + range_rate: -6.02588 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 60 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: 0.628646 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.124704 + elevation_angle_std: 0.0731464 + range: 12.3313 + range_std: 0.200000 + range_rate: -5.00130 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 64 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 11 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.168508 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0129626 + elevation_angle_std: 0.0706906 + range: 12.4147 + range_std: 0.200000 + range_rate: -6.04737 + range_rate_std: 0.250000 + rcs: -12 + measurement_id: 63 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 25 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: -0.159851 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0418398 + elevation_angle_std: 0.0731464 + range: 12.9576 + range_std: 0.200000 + range_rate: -6.05502 + range_rate_std: 0.250000 + rcs: -8 + measurement_id: 66 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 9 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: 0.926770 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00981561 + elevation_angle_std: 0.0731464 + range: 13.5961 + range_std: 0.200000 + range_rate: -3.79887 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 70 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 713 + ambiguity_flag: 100 + - azimuth_angle: -0.145241 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0441899 + elevation_angle_std: 0.0731464 + range: 13.7677 + range_std: 0.200000 + range_rate: -6.08591 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 69 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: 0.872911 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0432329 + elevation_angle_std: 0.0731464 + range: 14.0529 + range_std: 0.200000 + range_rate: -4.05713 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 77 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 22 + object_id: 713 + ambiguity_flag: 100 + - azimuth_angle: 0.979323 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0108081 + elevation_angle_std: 0.0731464 + range: 14.0719 + range_std: 0.200000 + range_rate: -3.53753 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 75 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 713 + ambiguity_flag: 100 + - azimuth_angle: -0.140049 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0112438 + elevation_angle_std: 0.0731464 + range: 14.1078 + range_std: 0.200000 + range_rate: -6.09649 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 74 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: 0.907153 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.124753 + elevation_angle_std: 0.0631005 + range: 14.3205 + range_std: 0.200000 + range_rate: -3.82649 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 76 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 713 + ambiguity_flag: 100 + - azimuth_angle: 0.862056 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.193373 + elevation_angle_std: 0.0731464 + range: 14.6090 + range_std: 0.200000 + range_rate: -4.10556 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 72 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 713 + ambiguity_flag: 100 + - azimuth_angle: 0.792535 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00631777 + elevation_angle_std: 0.0731464 + range: 14.9213 + range_std: 0.200000 + range_rate: -4.39365 + range_rate_std: 0.250000 + rcs: -15 + measurement_id: 79 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 15 + object_id: 713 + ambiguity_flag: 0 + - azimuth_angle: -0.123849 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0169206 + elevation_angle_std: 0.0387393 + range: 15.2637 + range_std: 0.200000 + range_rate: -6.11047 + range_rate_std: 0.250000 + rcs: 0 + measurement_id: 80 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 7867 + ambiguity_flag: 100 + - azimuth_angle: 0.846611 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0596542 + elevation_angle_std: 0.0563254 + range: 16.1923 + range_std: 0.200000 + range_rate: -4.15638 + range_rate_std: 0.250000 + rcs: -12 + measurement_id: 81 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 713 + ambiguity_flag: 70 + - azimuth_angle: -0.103894 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.168604 + elevation_angle_std: 0.0731464 + range: 17.0493 + range_std: 0.200000 + range_rate: -6.11955 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 83 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: 0.817138 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0909819 + elevation_angle_std: 0.0387393 + range: 18.3839 + range_std: 0.200000 + range_rate: -4.25468 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 90 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 713 + ambiguity_flag: 0 + - azimuth_angle: -0.0865819 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.105953 + elevation_angle_std: 0.0731464 + range: 18.7582 + range_std: 0.200000 + range_rate: -6.11656 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 93 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 10 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.626937 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0606606 + elevation_angle_std: 0.0731464 + range: 19.2246 + range_std: 0.200000 + range_rate: -4.99793 + range_rate_std: 0.250000 + rcs: -20 + measurement_id: 91 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 14 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.0813024 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00204542 + elevation_angle_std: 0.0631005 + range: 19.2271 + range_std: 0.200000 + range_rate: -6.13777 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 94 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.0816866 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00507115 + elevation_angle_std: 0.0731464 + range: 19.2371 + range_std: 0.200000 + range_rate: -6.14606 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 99 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.635539 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0622248 + elevation_angle_std: 0.0731464 + range: 19.2622 + range_std: 0.200000 + range_rate: -4.95984 + range_rate_std: 0.250000 + rcs: -19 + measurement_id: 95 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 58 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.761617 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0579811 + elevation_angle_std: 0.0448793 + range: 19.2869 + range_std: 0.200000 + range_rate: -4.50011 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 98 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 6679 + ambiguity_flag: 100 + - azimuth_angle: -0.438711 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0314243 + elevation_angle_std: 0.0387393 + range: 19.5582 + range_std: 0.200000 + range_rate: -5.54400 + range_rate_std: 0.250000 + rcs: -18 + measurement_id: 97 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 36 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.576625 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0876160 + elevation_angle_std: 0.0264734 + range: 19.6776 + range_std: 0.200000 + range_rate: -5.11458 + range_rate_std: 0.250000 + rcs: -12 + measurement_id: 96 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 1 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.472665 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0284820 + elevation_angle_std: 0.0285679 + range: 20.3352 + range_std: 0.200000 + range_rate: -5.44586 + range_rate_std: 0.250000 + rcs: -14 + measurement_id: 101 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 11 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: -0.496033 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0277676 + elevation_angle_std: 0.0502777 + range: 20.4031 + range_std: 0.200000 + range_rate: -5.36953 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 100 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 42 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: -0.0698347 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00862948 + elevation_angle_std: 0.0502777 + range: 20.5385 + range_std: 0.200000 + range_rate: -6.14562 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 103 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 6 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.0702918 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0124542 + elevation_angle_std: 0.0563254 + range: 20.8742 + range_std: 0.200000 + range_rate: -6.14942 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 104 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 9 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: 0.624895 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.139392 + elevation_angle_std: 0.0731464 + range: 21.4991 + range_std: 0.200000 + range_rate: -4.99841 + range_rate_std: 0.250000 + rcs: -15 + measurement_id: 105 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 16 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.901371 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00992453 + elevation_angle_std: 0.0631005 + range: 21.5062 + range_std: 0.200000 + range_rate: -51.5917 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 109 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 23 + object_id: 0 + ambiguity_flag: 30 + - azimuth_angle: 0.603751 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00442970 + elevation_angle_std: 0.0563254 + range: 21.7314 + range_std: 0.200000 + range_rate: -5.11753 + range_rate_std: 0.250000 + rcs: -18 + measurement_id: 106 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 6 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.0602114 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00626935 + elevation_angle_std: 0.0731464 + range: 21.8693 + range_std: 0.200000 + range_rate: -6.14599 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 110 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.0612227 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0183248 + elevation_angle_std: 0.0731464 + range: 22.2927 + range_std: 0.200000 + range_rate: -6.14968 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 111 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: -0.0535213 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0142214 + elevation_angle_std: 0.0563254 + range: 23.2965 + range_std: 0.200000 + range_rate: -5.84218 + range_rate_std: 0.250000 + rcs: -19 + measurement_id: 117 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 7 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.0587232 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.169906 + elevation_angle_std: 0.0387393 + range: 23.3159 + range_std: 0.200000 + range_rate: -6.16014 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 115 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 12954 + ambiguity_flag: 100 + - azimuth_angle: 0.842577 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0111833 + elevation_angle_std: 0.0180912 + range: 23.7689 + range_std: 0.200000 + range_rate: -4.18401 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 116 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 3 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.486449 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00968248 + elevation_angle_std: 0.0731464 + range: 23.8923 + range_std: 0.200000 + range_rate: -5.49272 + range_rate_std: 0.250000 + rcs: -20 + measurement_id: 120 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 15 + object_id: 0 + ambiguity_flag: 50 + - azimuth_angle: -0.460981 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0180706 + elevation_angle_std: 0.0731464 + range: 24.0164 + range_std: 0.200000 + range_rate: -5.40490 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 119 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 18 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.838989 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0114254 + elevation_angle_std: 0.0155357 + range: 24.3818 + range_std: 0.200000 + range_rate: -4.19404 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 121 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 0 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.951699 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00583364 + elevation_angle_std: 0.0308281 + range: 25.1034 + range_std: 0.200000 + range_rate: -5.90073 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 122 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 22 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.441023 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.129133 + elevation_angle_std: 0.0418043 + range: 25.1791 + range_std: 0.200000 + range_rate: -5.48306 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 127 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 40 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.891816 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00272318 + elevation_angle_std: 0.0731464 + range: 25.1950 + range_std: 0.200000 + range_rate: -5.88311 + range_rate_std: 0.250000 + rcs: 0 + measurement_id: 125 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 9 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.0417845 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00769753 + elevation_angle_std: 0.0502777 + range: 25.5056 + range_std: 0.200000 + range_rate: -6.16231 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 126 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: 0.445642 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00544634 + elevation_angle_std: 0.0285679 + range: 25.8673 + range_std: 0.200000 + range_rate: -5.62384 + range_rate_std: 0.250000 + rcs: -13 + measurement_id: 128 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 10 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.575884 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.134688 + elevation_angle_std: 0.0731464 + range: 26.2213 + range_std: 0.200000 + range_rate: -5.17744 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 132 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 15 + object_id: 13504 + ambiguity_flag: 100 + - azimuth_angle: -0.0401769 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0177195 + elevation_angle_std: 0.0358991 + range: 26.3633 + range_std: 0.200000 + range_rate: -6.14087 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 131 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: -0.357954 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0168722 + elevation_angle_std: 0.0731464 + range: 26.4201 + range_std: 0.200000 + range_rate: -5.92728 + range_rate_std: 0.250000 + rcs: -14 + measurement_id: 135 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 8 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.435498 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00424815 + elevation_angle_std: 0.0332671 + range: 26.8043 + range_std: 0.200000 + range_rate: -5.65915 + range_rate_std: 0.250000 + rcs: -14 + measurement_id: 133 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 18 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: -0.0396069 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0205037 + elevation_angle_std: 0.0631005 + range: 26.9237 + range_std: 0.200000 + range_rate: -6.12932 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 136 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: 0.569832 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.141116 + elevation_angle_std: 0.0731464 + range: 27.1449 + range_std: 0.200000 + range_rate: -5.17968 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 138 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 3 + object_id: 10568 + ambiguity_flag: 100 + - azimuth_angle: 0.297478 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00185177 + elevation_angle_std: 0.0731464 + range: 27.9333 + range_std: 0.200000 + range_rate: -5.93186 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 139 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 1602 + ambiguity_flag: 100 + - azimuth_angle: -0.0320342 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0744653 + elevation_angle_std: 0.0731464 + range: 28.6754 + range_std: 0.200000 + range_rate: -6.14178 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 142 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: 0.281549 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0109533 + elevation_angle_std: 0.0502777 + range: 28.8846 + range_std: 0.200000 + range_rate: -5.95295 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 141 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 1602 + ambiguity_flag: 100 + - azimuth_angle: 0.506676 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.135226 + elevation_angle_std: 0.0731464 + range: 28.9517 + range_std: 0.200000 + range_rate: -5.37398 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 140 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.465909 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0847855 + elevation_angle_std: 0.0195225 + range: 29.2955 + range_std: 0.200000 + range_rate: -5.53635 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 143 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 14 + object_id: 13504 + ambiguity_flag: 0 + - azimuth_angle: 0.496526 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.150682 + elevation_angle_std: 0.00872663 + range: 29.8373 + range_std: 0.200000 + range_rate: -5.39338 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 144 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 6 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.0254646 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00791538 + elevation_angle_std: 0.0195225 + range: 30.2011 + range_std: 0.200000 + range_rate: -6.13473 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 145 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: 0.270491 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0197411 + elevation_angle_std: 0.0448793 + range: 30.5430 + range_std: 0.200000 + range_rate: -5.97721 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 146 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 1602 + ambiguity_flag: 100 + - azimuth_angle: 0.490870 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.138928 + elevation_angle_std: 0.0210671 + range: 31.5646 + range_std: 0.200000 + range_rate: -5.43641 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 147 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 0 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.491769 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.139514 + elevation_angle_std: 0.0227338 + range: 31.7313 + range_std: 0.200000 + range_rate: -5.40650 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 150 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 8 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.0201904 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00596678 + elevation_angle_std: 0.0563254 + range: 31.8285 + range_std: 0.200000 + range_rate: -6.17178 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 151 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: -0.554165 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0543930 + elevation_angle_std: 0.0731464 + range: 31.9865 + range_std: 0.200000 + range_rate: -5.17744 + range_rate_std: 0.250000 + rcs: -16 + measurement_id: 149 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 13 + object_id: 0 + ambiguity_flag: 50 + - azimuth_angle: -0.0190728 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0603332 + elevation_angle_std: 0.0387393 + range: 32.4135 + range_std: 0.200000 + range_rate: -6.15768 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 154 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 15335 + ambiguity_flag: 100 + - azimuth_angle: 0.258287 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0216537 + elevation_angle_std: 0.0227338 + range: 32.4524 + range_std: 0.200000 + range_rate: -5.99054 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 153 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 1602 + ambiguity_flag: 100 + - azimuth_angle: 0.484173 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.142852 + elevation_angle_std: 0.0731464 + range: 32.6326 + range_std: 0.200000 + range_rate: -5.43923 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 152 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 25 + object_id: 3392 + ambiguity_flag: 100 + - azimuth_angle: 0.440061 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0727420 + elevation_angle_std: 0.0285679 + range: 33.1965 + range_std: 0.200000 + range_rate: -5.61675 + range_rate_std: 0.250000 + rcs: -14 + measurement_id: 155 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 3392 + ambiguity_flag: 100 + - azimuth_angle: -0.644198 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0596663 + elevation_angle_std: 0.0731464 + range: 34.0819 + range_std: 0.200000 + range_rate: -4.88750 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 158 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 1001 + ambiguity_flag: 100 + - azimuth_angle: -0.0152053 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0582236 + elevation_angle_std: 0.0631005 + range: 34.1793 + range_std: 0.200000 + range_rate: -6.15559 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 157 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 16221 + ambiguity_flag: 100 + - azimuth_angle: -0.285555 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0940327 + elevation_angle_std: 0.0731464 + range: 34.3098 + range_std: 0.200000 + range_rate: -5.85555 + range_rate_std: 0.250000 + rcs: -12 + measurement_id: 161 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 24 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.445971 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.136228 + elevation_angle_std: 0.0731464 + range: 34.4080 + range_std: 0.200000 + range_rate: -5.60552 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 160 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 24 + object_id: 3392 + ambiguity_flag: 100 + - azimuth_angle: 0.900121 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.137596 + elevation_angle_std: 0.0358991 + range: 34.6507 + range_std: 0.200000 + range_rate: -5.39175 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 159 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 52 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: 0.425366 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0407496 + elevation_angle_std: 0.0308281 + range: 35.0093 + range_std: 0.200000 + range_rate: -5.31929 + range_rate_std: 0.250000 + rcs: -14 + measurement_id: 165 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 71 + object_id: 3392 + ambiguity_flag: 0 + - azimuth_angle: 0.411162 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0398048 + elevation_angle_std: 0.0308281 + range: 35.1414 + range_std: 0.200000 + range_rate: -5.49355 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 166 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 10 + object_id: 3392 + ambiguity_flag: 0 + - azimuth_angle: 0.543941 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0447351 + elevation_angle_std: 0.0210671 + range: 35.1525 + range_std: 0.200000 + range_rate: -5.72329 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 167 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 19 + object_id: 3392 + ambiguity_flag: 0 + - azimuth_angle: -0.596900 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0586479 + elevation_angle_std: 0.0731464 + range: 35.4804 + range_std: 0.200000 + range_rate: -5.03786 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 163 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 25 + object_id: 1001 + ambiguity_flag: 100 + - azimuth_angle: -0.578951 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0674279 + elevation_angle_std: 0.0731464 + range: 35.4816 + range_std: 0.200000 + range_rate: -5.08151 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 164 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 61 + object_id: 1001 + ambiguity_flag: 0 + - azimuth_angle: -0.00948288 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.000629381 + elevation_angle_std: 0.0133411 + range: 35.5311 + range_std: 0.200000 + range_rate: -6.14892 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 162 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 16221 + ambiguity_flag: 100 + - azimuth_angle: -0.591792 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0644562 + elevation_angle_std: 0.0731464 + range: 35.7125 + range_std: 0.200000 + range_rate: -5.05471 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 169 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 61 + object_id: 1001 + ambiguity_flag: 0 + - azimuth_angle: -0.00667963 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00624515 + elevation_angle_std: 0.0731464 + range: 36.1959 + range_std: 0.200000 + range_rate: -6.21894 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 171 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 16221 + ambiguity_flag: 100 + - azimuth_angle: -0.263450 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0834009 + elevation_angle_std: 0.0332671 + range: 36.5867 + range_std: 0.200000 + range_rate: -5.89156 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 170 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 4 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.517829 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0571083 + elevation_angle_std: 0.0731464 + range: 36.6288 + range_std: 0.200000 + range_rate: -5.26220 + range_rate_std: 0.250000 + rcs: -17 + measurement_id: 173 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 23 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: -0.529631 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.133821 + elevation_angle_std: 0.0731464 + range: 36.7330 + range_std: 0.200000 + range_rate: -5.14154 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 172 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 1001 + ambiguity_flag: 100 + - azimuth_angle: -0.00570814 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00583364 + elevation_angle_std: 0.0285679 + range: 37.5294 + range_std: 0.200000 + range_rate: -6.18450 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 174 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 16221 + ambiguity_flag: 100 + - azimuth_angle: 0.479731 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.130159 + elevation_angle_std: 0.0210671 + range: 37.9691 + range_std: 0.200000 + range_rate: -5.46115 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 175 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 5 + object_id: 0 + ambiguity_flag: 70 + - azimuth_angle: 0.375398 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0108565 + elevation_angle_std: 0.0731464 + range: 38.3199 + range_std: 0.200000 + range_rate: -5.78239 + range_rate_std: 0.250000 + rcs: 15 + measurement_id: 176 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 5 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.228594 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0168359 + elevation_angle_std: 0.00872663 + range: 38.4465 + range_std: 0.200000 + range_rate: -6.03451 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 177 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 4889 + ambiguity_flag: 100 + - azimuth_angle: 0.173981 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0107113 + elevation_angle_std: 0.0167648 + range: 39.2612 + range_std: 0.200000 + range_rate: -6.09212 + range_rate_std: 0.250000 + rcs: 15 + measurement_id: 178 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 16837 + ambiguity_flag: 100 + - azimuth_angle: 0.155488 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.209295 + elevation_angle_std: 0.0502777 + range: 40.1003 + range_std: 0.200000 + range_rate: -5.12028 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 179 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 16837 + ambiguity_flag: 100 + - azimuth_angle: 0.355664 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00237219 + elevation_angle_std: 0.0245324 + range: 40.3722 + range_std: 0.200000 + range_rate: -5.82460 + range_rate_std: 0.250000 + rcs: 34 + measurement_id: 180 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 9946 + ambiguity_flag: 100 + - azimuth_angle: 0.442164 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.184156 + elevation_angle_std: 0.0308281 + range: 41.0726 + range_std: 0.200000 + range_rate: -5.48927 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 181 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 14 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.181470 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0267869 + elevation_angle_std: 0.00872663 + range: 41.1728 + range_std: 0.200000 + range_rate: -6.07802 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 183 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 30 + object_id: 16837 + ambiguity_flag: 100 + - azimuth_angle: 0.319667 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00729812 + elevation_angle_std: 0.0180912 + range: 41.1734 + range_std: 0.200000 + range_rate: -5.88282 + range_rate_std: 0.250000 + rcs: 15 + measurement_id: 182 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 18302 + ambiguity_flag: 100 + - azimuth_angle: 0.465677 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0713829 + elevation_angle_std: 0.0308281 + range: 41.6060 + range_std: 0.200000 + range_rate: -5.55564 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 184 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 8 + object_id: 9946 + ambiguity_flag: 0 + - azimuth_angle: 0.00439895 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00410292 + elevation_angle_std: 0.0502777 + range: 42.2423 + range_std: 0.200000 + range_rate: -6.17940 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 186 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 16221 + ambiguity_flag: 100 + - azimuth_angle: -0.197103 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0950174 + elevation_angle_std: 0.0502777 + range: 42.7520 + range_std: 0.200000 + range_rate: -5.96704 + range_rate_std: 0.250000 + rcs: -4 + measurement_id: 187 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 40 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.311905 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0156134 + elevation_angle_std: 0.0731464 + range: 43.6599 + range_std: 0.200000 + range_rate: -5.88965 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 190 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 48 + object_id: 18302 + ambiguity_flag: 100 + - azimuth_angle: -0.859481 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0937896 + elevation_angle_std: 0.0731464 + range: 43.7383 + range_std: 0.200000 + range_rate: -5.68662 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 189 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 39 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.00604102 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00565209 + elevation_angle_std: 0.0210671 + range: 44.1654 + range_std: 0.200000 + range_rate: -6.16936 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 191 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 9359 + ambiguity_flag: 100 + - azimuth_angle: 0.470540 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.109179 + elevation_angle_std: 0.0731464 + range: 44.9854 + range_std: 0.200000 + range_rate: -5.52501 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 193 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 40 + object_id: 1332 + ambiguity_flag: 100 + - azimuth_angle: 0.313011 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.198962 + elevation_angle_std: 0.0731464 + range: 45.3581 + range_std: 0.200000 + range_rate: -5.91636 + range_rate_std: 0.250000 + rcs: 25 + measurement_id: 196 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 18302 + ambiguity_flag: 100 + - azimuth_angle: 0.459708 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.194853 + elevation_angle_std: 0.0731464 + range: 45.7442 + range_std: 0.200000 + range_rate: -5.41260 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 195 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 16 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.212970 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.118888 + elevation_angle_std: 0.0731464 + range: 46.0724 + range_std: 0.200000 + range_rate: -5.99300 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 198 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 27 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.00942864 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00187598 + elevation_angle_std: 0.0227338 + range: 46.3537 + range_std: 0.200000 + range_rate: -6.17141 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 200 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 9359 + ambiguity_flag: 100 + - azimuth_angle: 0.232810 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.119595 + elevation_angle_std: 0.0418043 + range: 46.7264 + range_std: 0.200000 + range_rate: -5.80873 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 202 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 29 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.303503 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.129707 + elevation_angle_std: 0.0731464 + range: 47.8071 + range_std: 0.200000 + range_rate: -5.85101 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 204 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 15 + object_id: 18334 + ambiguity_flag: 0 + - azimuth_angle: 0.219983 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.115610 + elevation_angle_std: 0.0731464 + range: 47.9790 + range_std: 0.200000 + range_rate: -6.04600 + range_rate_std: 0.250000 + rcs: -4 + measurement_id: 206 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 71 + object_id: 0 + ambiguity_flag: 90 + - azimuth_angle: 0.284569 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.190179 + elevation_angle_std: 0.0731464 + range: 48.1003 + range_std: 0.200000 + range_rate: -5.95143 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 208 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 18302 + ambiguity_flag: 100 + - azimuth_angle: 0.0126015 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00635408 + elevation_angle_std: 0.0502777 + range: 48.6437 + range_std: 0.200000 + range_rate: -6.16917 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 209 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 6 + object_id: 9359 + ambiguity_flag: 100 + - azimuth_angle: 0.331167 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.104335 + elevation_angle_std: 0.0706906 + range: 48.7715 + range_std: 0.200000 + range_rate: -5.80126 + range_rate_std: 0.250000 + rcs: -8 + measurement_id: 211 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 34 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.475261 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.110859 + elevation_angle_std: 0.0563254 + range: 49.0759 + range_std: 0.200000 + range_rate: -5.47826 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 210 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 16 + object_id: 9615 + ambiguity_flag: 100 + - azimuth_angle: 0.0136901 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00398189 + elevation_angle_std: 0.0264734 + range: 49.9099 + range_std: 0.200000 + range_rate: -6.17831 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 215 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 33 + object_id: 9359 + ambiguity_flag: 100 + - azimuth_angle: 0.326381 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0162912 + elevation_angle_std: 0.0387393 + range: 50.3191 + range_std: 0.200000 + range_rate: -5.88363 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 213 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 4 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.274366 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00626935 + elevation_angle_std: 0.0731464 + range: 50.4753 + range_std: 0.200000 + range_rate: -5.97189 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 214 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 13 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.263748 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00754018 + elevation_angle_std: 0.0195225 + range: 51.3725 + range_std: 0.200000 + range_rate: -5.99124 + range_rate_std: 0.250000 + rcs: 19 + measurement_id: 216 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.0168371 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0112438 + elevation_angle_std: 0.0631005 + range: 52.1210 + range_std: 0.200000 + range_rate: -6.16810 + range_rate_std: 0.250000 + rcs: -8 + measurement_id: 217 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 17 + object_id: 10561 + ambiguity_flag: 100 + - azimuth_angle: 0.262348 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0163154 + elevation_angle_std: 0.0706906 + range: 52.5039 + range_std: 0.200000 + range_rate: -5.98603 + range_rate_std: 0.250000 + rcs: 9 + measurement_id: 218 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 6057 + ambiguity_flag: 0 + - azimuth_angle: 0.0185324 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00790328 + elevation_angle_std: 0.0264734 + range: 54.3746 + range_std: 0.200000 + range_rate: -6.17032 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 220 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 10561 + ambiguity_flag: 100 + - azimuth_angle: 0.137443 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0277071 + elevation_angle_std: 0.0210671 + range: 55.0266 + range_std: 0.200000 + range_rate: -6.11856 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 222 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 17101 + ambiguity_flag: 100 + - azimuth_angle: 0.0207204 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00259005 + elevation_angle_std: 0.0167648 + range: 56.0597 + range_std: 0.200000 + range_rate: -6.16925 + range_rate_std: 0.250000 + rcs: 9 + measurement_id: 224 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 10561 + ambiguity_flag: 100 + - azimuth_angle: 0.251427 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0142698 + elevation_angle_std: 0.0143966 + range: 56.4420 + range_std: 0.200000 + range_rate: -6.00382 + range_rate_std: 0.250000 + rcs: 17 + measurement_id: 225 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 6057 + ambiguity_flag: 100 + - azimuth_angle: 0.0209393 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00130714 + elevation_angle_std: 0.0114566 + range: 56.8565 + range_std: 0.200000 + range_rate: -6.15759 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 226 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 10561 + ambiguity_flag: 100 + - azimuth_angle: 0.0214076 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0114859 + elevation_angle_std: 0.0731464 + range: 57.9551 + range_std: 0.200000 + range_rate: -6.19287 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 227 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 10561 + ambiguity_flag: 100 + - azimuth_angle: 0.0221142 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00889575 + elevation_angle_std: 0.0731464 + range: 58.3910 + range_std: 0.200000 + range_rate: -6.16887 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 229 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 3918 + ambiguity_flag: 100 + - azimuth_angle: 0.248309 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0198742 + elevation_angle_std: 0.0731464 + range: 59.8972 + range_std: 0.200000 + range_rate: -6.00577 + range_rate_std: 0.250000 + rcs: -11 + measurement_id: 233 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 17 + object_id: 6057 + ambiguity_flag: 0 + - azimuth_angle: -0.515096 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00242060 + elevation_angle_std: 0.0563254 + range: 60.4088 + range_std: 0.200000 + range_rate: -5.85644 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 234 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 11 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.253830 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.115037 + elevation_angle_std: 0.0731464 + range: 61.0239 + range_std: 0.200000 + range_rate: -5.99089 + range_rate_std: 0.250000 + rcs: -9 + measurement_id: 235 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 4 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.0236283 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0117159 + elevation_angle_std: 0.0731464 + range: 61.1869 + range_std: 0.200000 + range_rate: -6.19859 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 236 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 3918 + ambiguity_flag: 100 + - azimuth_angle: 0.0240084 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00815745 + elevation_angle_std: 0.0631005 + range: 61.9288 + range_std: 0.200000 + range_rate: -6.17804 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 237 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 3918 + ambiguity_flag: 100 + - azimuth_angle: 0.462236 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.115281 + elevation_angle_std: 0.0731464 + range: 62.3845 + range_std: 0.200000 + range_rate: -5.50774 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 240 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 24 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.0253167 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00641459 + elevation_angle_std: 0.0358991 + range: 62.5151 + range_std: 0.200000 + range_rate: -6.16265 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 239 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 3918 + ambiguity_flag: 100 + - azimuth_angle: 0.0270556 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00435707 + elevation_angle_std: 0.0731464 + range: 64.2294 + range_std: 0.200000 + range_rate: -6.16219 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 242 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 3918 + ambiguity_flag: 100 + - azimuth_angle: -0.382681 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0637528 + elevation_angle_std: 0.0731464 + range: 65.3856 + range_std: 0.200000 + range_rate: -5.80686 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 243 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 13 + object_id: 328 + ambiguity_flag: 100 + - azimuth_angle: -0.394032 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0412462 + elevation_angle_std: 0.0563254 + range: 65.4905 + range_std: 0.200000 + range_rate: -5.90670 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 244 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 13 + object_id: 328 + ambiguity_flag: 100 + - azimuth_angle: 0.0283335 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00321940 + elevation_angle_std: 0.0731464 + range: 68.2370 + range_std: 0.200000 + range_rate: -6.18257 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 246 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.0296260 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00000 + elevation_angle_std: 0.0308281 + range: 68.8237 + range_std: 0.200000 + range_rate: -6.18222 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 247 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.0310232 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00513167 + elevation_angle_std: 0.0308281 + range: 69.9063 + range_std: 0.200000 + range_rate: -6.16693 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 248 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.0310292 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00940410 + elevation_angle_std: 0.0563254 + range: 70.2073 + range_std: 0.200000 + range_rate: -6.17292 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 250 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.789814 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0203342 + elevation_angle_std: 0.0631005 + range: 70.3345 + range_std: 0.200000 + range_rate: -5.99977 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 249 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 11 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.0304675 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.000786712 + elevation_angle_std: 0.0155357 + range: 71.1310 + range_std: 0.200000 + range_rate: -6.17018 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 251 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.0295403 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00929517 + elevation_angle_std: 0.0180912 + range: 72.2124 + range_std: 0.200000 + range_rate: -6.16961 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 252 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 12324 + ambiguity_flag: 100 + - azimuth_angle: 0.0565612 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00929517 + elevation_angle_std: 0.0180912 + range: 72.2124 + range_std: 0.200000 + range_rate: -6.16961 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 256 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.0588275 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0185185 + elevation_angle_std: 0.0563254 + range: 73.1064 + range_std: 0.200000 + range_rate: -6.16776 + range_rate_std: 0.250000 + rcs: 24 + measurement_id: 253 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.0751432 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0185185 + elevation_angle_std: 0.0563254 + range: 73.1064 + range_std: 0.200000 + range_rate: -6.16776 + range_rate_std: 0.250000 + rcs: 19 + measurement_id: 257 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.0703182 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0247893 + elevation_angle_std: 0.0502777 + range: 73.6103 + range_std: 0.200000 + range_rate: -6.15690 + range_rate_std: 0.250000 + rcs: 20 + measurement_id: 254 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 62 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.0683437 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0126358 + elevation_angle_std: 0.0114566 + range: 74.4475 + range_std: 0.200000 + range_rate: -6.15829 + range_rate_std: 0.250000 + rcs: 23 + measurement_id: 255 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 18 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.139083 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0449410 + elevation_angle_std: 0.0731464 + range: 76.0308 + range_std: 0.200000 + range_rate: -6.12395 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 259 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 16 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.0673893 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0215690 + elevation_angle_std: 0.0285679 + range: 76.2638 + range_std: 0.200000 + range_rate: -6.17915 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 260 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 26 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.244739 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0115827 + elevation_angle_std: 0.0180912 + range: 76.6624 + range_std: 0.200000 + range_rate: -6.00867 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 261 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 17 + object_id: 0 + ambiguity_flag: 50 + - azimuth_angle: 0.0686927 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0190148 + elevation_angle_std: 0.0563254 + range: 76.9317 + range_std: 0.200000 + range_rate: -6.16115 + range_rate_std: 0.250000 + rcs: 8 + measurement_id: 262 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 7280 + ambiguity_flag: 100 + - azimuth_angle: 0.225147 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00543425 + elevation_angle_std: 0.0502777 + range: 77.9674 + range_std: 0.200000 + range_rate: -6.03697 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 263 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 5 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.0359177 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.167168 + elevation_angle_std: 0.0731464 + range: 78.2274 + range_std: 0.200000 + range_rate: -6.17040 + range_rate_std: 0.250000 + rcs: 14 + measurement_id: 264 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 11 + object_id: 13506 + ambiguity_flag: 70 + - azimuth_angle: 0.220591 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.178397 + elevation_angle_std: 0.0731464 + range: 80.2086 + range_std: 0.200000 + range_rate: -6.03748 + range_rate_std: 0.250000 + rcs: 17 + measurement_id: 265 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 23112 + ambiguity_flag: 50 + - azimuth_angle: 0.0354684 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0322598 + elevation_angle_std: 0.0731464 + range: 80.2615 + range_std: 0.200000 + range_rate: -6.17094 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 266 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 13506 + ambiguity_flag: 100 + - azimuth_angle: 0.216567 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0101303 + elevation_angle_std: 0.0448793 + range: 82.2147 + range_std: 0.200000 + range_rate: -6.05420 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 267 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 23112 + ambiguity_flag: 100 + - azimuth_angle: 0.0365073 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00350987 + elevation_angle_std: 0.0731464 + range: 82.2440 + range_std: 0.200000 + range_rate: -6.16983 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 268 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 13506 + ambiguity_flag: 100 + - azimuth_angle: 0.0368321 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00515587 + elevation_angle_std: 0.0563254 + range: 83.4106 + range_std: 0.200000 + range_rate: -6.17295 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 269 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 13506 + ambiguity_flag: 100 + - azimuth_angle: 0.0361811 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00697133 + elevation_angle_std: 0.0448793 + range: 83.8965 + range_std: 0.200000 + range_rate: -6.15873 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 270 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 13506 + ambiguity_flag: 100 + - azimuth_angle: 0.0364119 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0243655 + elevation_angle_std: 0.0731464 + range: 84.7085 + range_std: 0.200000 + range_rate: -6.17182 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 271 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 13506 + ambiguity_flag: 100 + - azimuth_angle: -0.198910 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00387296 + elevation_angle_std: 0.0308281 + range: 86.0150 + range_std: 0.200000 + range_rate: -5.81138 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 273 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 36 + object_id: 8776 + ambiguity_flag: 100 + - azimuth_angle: 0.210755 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0106507 + elevation_angle_std: 0.0285679 + range: 86.2261 + range_std: 0.200000 + range_rate: -6.04584 + range_rate_std: 0.250000 + rcs: -5 + measurement_id: 272 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 9 + object_id: 23112 + ambiguity_flag: 100 + - azimuth_angle: 0.0523298 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0837652 + elevation_angle_std: 0.0706906 + range: 87.3928 + range_std: 0.200000 + range_rate: -6.15084 + range_rate_std: 0.250000 + rcs: 26 + measurement_id: 275 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 22 + object_id: 15044 + ambiguity_flag: 100 + - azimuth_angle: -0.0680339 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0388722 + elevation_angle_std: 0.0210671 + range: 88.4314 + range_std: 0.200000 + range_rate: -6.16592 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 276 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 22 + object_id: 17741 + ambiguity_flag: 100 + - azimuth_angle: -0.0453084 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0412826 + elevation_angle_std: 0.0731464 + range: 96.1103 + range_std: 0.200000 + range_rate: -6.13311 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 283 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 23420 + ambiguity_flag: 100 + - azimuth_angle: 0.227686 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.153682 + elevation_angle_std: 0.0731464 + range: 96.9676 + range_std: 0.200000 + range_rate: -5.97854 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 284 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 7875 + ambiguity_flag: 90 + - azimuth_angle: 0.227369 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0312790 + elevation_angle_std: 0.0731464 + range: 97.3215 + range_std: 0.200000 + range_rate: -6.05794 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 285 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 33 + object_id: 7875 + ambiguity_flag: 90 + - azimuth_angle: 0.227500 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0460678 + elevation_angle_std: 0.0227338 + range: 97.9109 + range_std: 0.200000 + range_rate: -6.02914 + range_rate_std: 0.250000 + rcs: 9 + measurement_id: 286 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 7 + object_id: 7875 + ambiguity_flag: 100 + - azimuth_angle: 0.212891 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0495938 + elevation_angle_std: 0.0731464 + range: 98.9328 + range_std: 0.200000 + range_rate: -6.04060 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 288 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 7 + object_id: 7875 + ambiguity_flag: 100 + - azimuth_angle: -0.412540 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0166059 + elevation_angle_std: 0.0731464 + range: 100.306 + range_std: 0.200000 + range_rate: -5.85203 + range_rate_std: 0.250000 + rcs: -10 + measurement_id: 289 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 16 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.108320 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0183248 + elevation_angle_std: 0.0731464 + range: 101.641 + range_std: 0.200000 + range_rate: -8.46414 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 291 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 6 + object_id: 3398 + ambiguity_flag: 100 + - azimuth_angle: 0.190886 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0275376 + elevation_angle_std: 0.0210671 + range: 104.295 + range_std: 0.200000 + range_rate: -6.07603 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 293 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 6062 + ambiguity_flag: 100 + - azimuth_angle: 0.219818 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0275376 + elevation_angle_std: 0.0210671 + range: 104.295 + range_std: 0.200000 + range_rate: -6.07603 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 313 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 6062 + ambiguity_flag: 100 + - azimuth_angle: -0.123365 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0296201 + elevation_angle_std: 0.0418043 + range: 105.419 + range_std: 0.200000 + range_rate: -6.07859 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 294 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 19 + object_id: 6062 + ambiguity_flag: 100 + - azimuth_angle: -0.0330138 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0367648 + elevation_angle_std: 0.0731464 + range: 109.025 + range_std: 0.200000 + range_rate: -6.12234 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 295 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 13 + object_id: 5788 + ambiguity_flag: 100 + - azimuth_angle: -0.0326211 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0404468 + elevation_angle_std: 0.0731464 + range: 109.455 + range_std: 0.200000 + range_rate: -6.15543 + range_rate_std: 0.250000 + rcs: -7 + measurement_id: 296 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 51 + object_id: 5788 + ambiguity_flag: 100 + - azimuth_angle: -0.0338834 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0327684 + elevation_angle_std: 0.0706906 + range: 110.065 + range_std: 0.200000 + range_rate: -6.19704 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 297 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 7 + object_id: 5788 + ambiguity_flag: 0 + - azimuth_angle: 0.212853 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0283972 + elevation_angle_std: 0.0631005 + range: 110.513 + range_std: 0.200000 + range_rate: -6.04608 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 298 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 9 + object_id: 7216 + ambiguity_flag: 0 + - azimuth_angle: 0.206509 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0206974 + elevation_angle_std: 0.0731464 + range: 112.674 + range_std: 0.200000 + range_rate: -6.05386 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 300 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 11 + object_id: 7216 + ambiguity_flag: 100 + - azimuth_angle: 0.535027 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.130879 + elevation_angle_std: 0.0731464 + range: 113.029 + range_std: 0.200000 + range_rate: -6.08417 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 301 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 26 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.121622 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0133015 + elevation_angle_std: 0.0180912 + range: 113.875 + range_std: 0.200000 + range_rate: -6.13807 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 303 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 0 + object_id: 14710 + ambiguity_flag: 100 + - azimuth_angle: 0.206654 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0438870 + elevation_angle_std: 0.0731464 + range: 114.050 + range_std: 0.200000 + range_rate: -6.05016 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 302 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 7216 + ambiguity_flag: 50 + - azimuth_angle: -0.0270182 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0336403 + elevation_angle_std: 0.0332671 + range: 115.765 + range_std: 0.200000 + range_rate: -6.17951 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 307 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 1607 + ambiguity_flag: 100 + - azimuth_angle: 0.201957 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0701332 + elevation_angle_std: 0.0731464 + range: 115.855 + range_std: 0.200000 + range_rate: -6.04483 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 306 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 6 + object_id: 7216 + ambiguity_flag: 100 + - azimuth_angle: 0.123166 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00562788 + elevation_angle_std: 0.0114566 + range: 116.947 + range_std: 0.200000 + range_rate: -6.14824 + range_rate_std: 0.250000 + rcs: 8 + measurement_id: 308 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 3 + object_id: 429 + ambiguity_flag: 100 + - azimuth_angle: 0.199668 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0749144 + elevation_angle_std: 0.0502777 + range: 118.510 + range_std: 0.200000 + range_rate: -6.05467 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 310 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 8 + object_id: 430 + ambiguity_flag: 100 + - azimuth_angle: 0.240364 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0194263 + elevation_angle_std: 0.0563254 + range: 121.692 + range_std: 0.200000 + range_rate: -6.05335 + range_rate_std: 0.250000 + rcs: -6 + measurement_id: 316 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 12 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.122259 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00611201 + elevation_angle_std: 0.0114566 + range: 122.419 + range_std: 0.200000 + range_rate: -6.13541 + range_rate_std: 0.250000 + rcs: 8 + measurement_id: 318 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 4 + object_id: 429 + ambiguity_flag: 100 + - azimuth_angle: 0.202875 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0324899 + elevation_angle_std: 0.0631005 + range: 123.112 + range_std: 0.200000 + range_rate: -6.04820 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 319 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 430 + ambiguity_flag: 100 + - azimuth_angle: 0.191692 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0803650 + elevation_angle_std: 0.0731464 + range: 123.797 + range_std: 0.200000 + range_rate: -6.03873 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 320 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 22 + object_id: 430 + ambiguity_flag: 100 + - azimuth_angle: -0.107671 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0157344 + elevation_angle_std: 0.0358991 + range: 124.180 + range_std: 0.200000 + range_rate: -6.10614 + range_rate_std: 0.250000 + rcs: 0 + measurement_id: 321 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 23 + object_id: 7577 + ambiguity_flag: 50 + - azimuth_angle: 0.124414 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00755229 + elevation_angle_std: 0.0195225 + range: 125.099 + range_std: 0.200000 + range_rate: -6.13034 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 322 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 8 + object_id: 27905 + ambiguity_flag: 100 + - azimuth_angle: 0.116436 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.000351019 + elevation_angle_std: 0.0245324 + range: 128.442 + range_std: 0.200000 + range_rate: -6.13580 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 323 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 27905 + ambiguity_flag: 100 + - azimuth_angle: 0.116323 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0108565 + elevation_angle_std: 0.0167648 + range: 131.343 + range_std: 0.200000 + range_rate: -6.13505 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 327 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 2 + object_id: 12952 + ambiguity_flag: 100 + - azimuth_angle: 0.179850 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0551445 + elevation_angle_std: 0.0731464 + range: 131.406 + range_std: 0.200000 + range_rate: -5.93698 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 328 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 13 + object_id: 10556 + ambiguity_flag: 100 + - azimuth_angle: 0.186404 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.000919844 + elevation_angle_std: 0.0731464 + range: 132.085 + range_std: 0.200000 + range_rate: -5.86567 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 329 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 11 + object_id: 10556 + ambiguity_flag: 100 + - azimuth_angle: 0.204937 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0214237 + elevation_angle_std: 0.0332671 + range: 132.501 + range_std: 0.200000 + range_rate: -6.05209 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 330 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 10 + object_id: 10556 + ambiguity_flag: 0 + - azimuth_angle: 0.200254 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0823807 + elevation_angle_std: 0.0180912 + range: 133.534 + range_std: 0.200000 + range_rate: -6.05028 + range_rate_std: 0.250000 + rcs: 3 + measurement_id: 331 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 16 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.238409 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0283488 + elevation_angle_std: 0.0418043 + range: 134.072 + range_std: 0.200000 + range_rate: -6.02854 + range_rate_std: 0.250000 + rcs: 0 + measurement_id: 333 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 14 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.114425 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0139309 + elevation_angle_std: 0.0210671 + range: 135.806 + range_std: 0.200000 + range_rate: -6.14004 + range_rate_std: 0.250000 + rcs: 0 + measurement_id: 335 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 12 + object_id: 1603 + ambiguity_flag: 100 + - azimuth_angle: 0.177756 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.184833 + elevation_angle_std: 0.0264734 + range: 135.929 + range_std: 0.200000 + range_rate: -5.96842 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 337 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 20 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.180907 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.188121 + elevation_angle_std: 0.0418043 + range: 136.107 + range_std: 0.200000 + range_rate: -5.97055 + range_rate_std: 0.250000 + rcs: 21 + measurement_id: 341 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 28 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.116677 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0210727 + elevation_angle_std: 0.0731464 + range: 137.368 + range_std: 0.200000 + range_rate: -6.15236 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 343 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 9 + object_id: 1603 + ambiguity_flag: 100 + - azimuth_angle: -0.494056 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.198444 + elevation_angle_std: 0.0227338 + range: 137.561 + range_std: 0.200000 + range_rate: -5.23369 + range_rate_std: 0.250000 + rcs: 17 + measurement_id: 344 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 728 + ambiguity_flag: 100 + - azimuth_angle: 0.111868 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00986403 + elevation_angle_std: 0.0731464 + range: 138.122 + range_std: 0.200000 + range_rate: -6.13253 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 345 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 1603 + ambiguity_flag: 100 + - azimuth_angle: -0.423998 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.183725 + elevation_angle_std: 0.0387393 + range: 138.146 + range_std: 0.200000 + range_rate: -5.47655 + range_rate_std: 0.250000 + rcs: 16 + measurement_id: 346 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 1004 + ambiguity_flag: 100 + - azimuth_angle: 0.173635 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.184107 + elevation_angle_std: 0.0731464 + range: 140.420 + range_std: 0.200000 + range_rate: -5.98393 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 349 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 3 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.0165741 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0456074 + elevation_angle_std: 0.0731464 + range: 141.358 + range_std: 0.200000 + range_rate: -5.57759 + range_rate_std: 0.250000 + rcs: -2 + measurement_id: 352 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 6 + object_id: 710 + ambiguity_flag: 0 + - azimuth_angle: -0.113045 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0856236 + elevation_angle_std: 0.00983826 + range: 142.983 + range_std: 0.200000 + range_rate: -6.09983 + range_rate_std: 0.250000 + rcs: 22 + measurement_id: 356 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 438 + ambiguity_flag: 100 + - azimuth_angle: -0.0108888 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0741011 + elevation_angle_std: 0.0448793 + range: 144.080 + range_std: 0.200000 + range_rate: -6.13138 + range_rate_std: 0.250000 + rcs: 1 + measurement_id: 359 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 710 + ambiguity_flag: 100 + - azimuth_angle: 0.524042 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0230701 + elevation_angle_std: 0.0731464 + range: 144.551 + range_std: 0.200000 + range_rate: -5.40333 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 361 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 0 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.0179876 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0559445 + elevation_angle_std: 0.0706906 + range: 145.218 + range_std: 0.200000 + range_rate: -6.23228 + range_rate_std: 0.250000 + rcs: -3 + measurement_id: 365 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 33 + object_id: 710 + ambiguity_flag: 100 + - azimuth_angle: -0.247088 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.198074 + elevation_angle_std: 0.0563254 + range: 146.091 + range_std: 0.200000 + range_rate: -5.82241 + range_rate_std: 0.250000 + rcs: 17 + measurement_id: 366 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 2796 + ambiguity_flag: 100 + - azimuth_angle: 0.166552 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.208540 + elevation_angle_std: 0.0731464 + range: 146.931 + range_std: 0.200000 + range_rate: -5.99272 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 368 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 18 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.110351 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0823200 + elevation_angle_std: 0.0143966 + range: 147.009 + range_std: 0.200000 + range_rate: -6.09916 + range_rate_std: 0.250000 + rcs: 26 + measurement_id: 369 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 711 + ambiguity_flag: 100 + - azimuth_angle: 0.125327 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00292893 + elevation_angle_std: 0.0245324 + range: 148.038 + range_std: 0.200000 + range_rate: -6.13504 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 372 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 79 + object_id: 734 + ambiguity_flag: 0 + - azimuth_angle: 0.129692 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0122605 + elevation_angle_std: 0.0563254 + range: 149.201 + range_std: 0.200000 + range_rate: -6.14008 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 374 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 11 + object_id: 734 + ambiguity_flag: 100 + - azimuth_angle: 0.172875 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.179123 + elevation_angle_std: 0.0731464 + range: 150.137 + range_std: 0.200000 + range_rate: -5.99562 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 375 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.109567 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0796243 + elevation_angle_std: 0.00872663 + range: 151.171 + range_std: 0.200000 + range_rate: -6.09705 + range_rate_std: 0.250000 + rcs: 29 + measurement_id: 376 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 6651 + ambiguity_flag: 100 + - azimuth_angle: 0.162668 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.140064 + elevation_angle_std: 0.0731464 + range: 152.319 + range_std: 0.200000 + range_rate: -6.03115 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 377 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 20 + object_id: 4586 + ambiguity_flag: 100 + - azimuth_angle: 0.109225 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00200911 + elevation_angle_std: 0.0133411 + range: 154.203 + range_std: 0.200000 + range_rate: -6.14887 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 380 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 31 + object_id: 3685 + ambiguity_flag: 100 + - azimuth_angle: 0.110242 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00133135 + elevation_angle_std: 0.0133411 + range: 154.237 + range_std: 0.200000 + range_rate: -6.14753 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 381 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 47 + object_id: 3685 + ambiguity_flag: 100 + - azimuth_angle: -0.111957 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0729119 + elevation_angle_std: 0.0285679 + range: 155.346 + range_std: 0.200000 + range_rate: -6.10946 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 382 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 27 + object_id: 23427 + ambiguity_flag: 100 + - azimuth_angle: 0.108492 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00837531 + elevation_angle_std: 0.0731464 + range: 155.698 + range_std: 0.200000 + range_rate: -6.14043 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 384 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 7 + object_id: 3685 + ambiguity_flag: 0 + - azimuth_angle: 0.162808 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.131501 + elevation_angle_std: 0.0731464 + range: 155.717 + range_std: 0.200000 + range_rate: -6.01763 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 383 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 25 + object_id: 4586 + ambiguity_flag: 100 + - azimuth_angle: 0.134201 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0139067 + elevation_angle_std: 0.0631005 + range: 156.309 + range_std: 0.200000 + range_rate: -6.12281 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 386 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 3685 + ambiguity_flag: 100 + - azimuth_angle: 0.109663 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0110986 + elevation_angle_std: 0.0731464 + range: 157.032 + range_std: 0.200000 + range_rate: -6.14092 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 387 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 12 + object_id: 3685 + ambiguity_flag: 0 + - azimuth_angle: 0.108027 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0309884 + elevation_angle_std: 0.0706906 + range: 157.900 + range_std: 0.200000 + range_rate: -6.13804 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 389 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 3685 + ambiguity_flag: 50 + - azimuth_angle: 0.0823725 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00804852 + elevation_angle_std: 0.0155357 + range: 158.836 + range_std: 0.200000 + range_rate: -6.15304 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 391 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 8 + object_id: 17725 + ambiguity_flag: 100 + - azimuth_angle: 0.109382 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00649931 + elevation_angle_std: 0.0448793 + range: 160.254 + range_std: 0.200000 + range_rate: -6.13884 + range_rate_std: 0.250000 + rcs: 6 + measurement_id: 392 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 6 + object_id: 7555 + ambiguity_flag: 100 + - azimuth_angle: 0.131509 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0136525 + elevation_angle_std: 0.0731464 + range: 161.553 + range_std: 0.200000 + range_rate: -6.13327 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 393 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 2 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.161570 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.186286 + elevation_angle_std: 0.0563254 + range: 165.019 + range_std: 0.200000 + range_rate: -6.00225 + range_rate_std: 0.250000 + rcs: 16 + measurement_id: 396 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 6081 + ambiguity_flag: 100 + - azimuth_angle: 0.162680 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.190870 + elevation_angle_std: 0.0731464 + range: 165.961 + range_std: 0.200000 + range_rate: -6.00413 + range_rate_std: 0.250000 + rcs: 21 + measurement_id: 397 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 3 + object_id: 0 + ambiguity_flag: 100 + - azimuth_angle: 0.133200 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0157586 + elevation_angle_std: 0.0563254 + range: 167.356 + range_std: 0.200000 + range_rate: -6.15034 + range_rate_std: 0.250000 + rcs: -1 + measurement_id: 399 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 17 + object_id: 13239 + ambiguity_flag: 70 + - azimuth_angle: 0.161752 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.183110 + elevation_angle_std: 0.0731464 + range: 167.422 + range_std: 0.200000 + range_rate: -5.99836 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 398 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 24 + object_id: 0 + ambiguity_flag: 70 + - azimuth_angle: 0.108385 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0338219 + elevation_angle_std: 0.0731464 + range: 172.054 + range_std: 0.200000 + range_rate: -6.14791 + range_rate_std: 0.250000 + rcs: 2 + measurement_id: 402 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 18 + object_id: 9958 + ambiguity_flag: 100 + - azimuth_angle: 0.158625 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0162065 + elevation_angle_std: 0.0731464 + range: 172.699 + range_std: 0.200000 + range_rate: -6.00471 + range_rate_std: 0.250000 + rcs: 8 + measurement_id: 403 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 1606 + ambiguity_flag: 100 + - azimuth_angle: 0.0481090 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00101667 + elevation_angle_std: 0.0133411 + range: 175.188 + range_std: 0.200000 + range_rate: -6.17080 + range_rate_std: 0.250000 + rcs: 17 + measurement_id: 404 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 5 + object_id: 14445 + ambiguity_flag: 100 + - azimuth_angle: 0.0162523 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0263995 + elevation_angle_std: 0.0358991 + range: 177.673 + range_std: 0.200000 + range_rate: -6.15984 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 405 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 17 + object_id: 1299 + ambiguity_flag: 100 + - azimuth_angle: 0.0444916 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0119821 + elevation_angle_std: 0.0706906 + range: 183.480 + range_std: 0.200000 + range_rate: -6.17502 + range_rate_std: 0.250000 + rcs: 13 + measurement_id: 406 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 53 + object_id: 24332 + ambiguity_flag: 100 + - azimuth_angle: 0.0673132 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0303587 + elevation_angle_std: 0.0731464 + range: 198.750 + range_std: 0.200000 + range_rate: -6.15774 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 407 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 14 + object_id: 9365 + ambiguity_flag: 100 + - azimuth_angle: 0.0728861 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0165454 + elevation_angle_std: 0.0563254 + range: 201.510 + range_std: 0.200000 + range_rate: -6.16182 + range_rate_std: 0.250000 + rcs: 20 + measurement_id: 408 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 3 + object_id: 9365 + ambiguity_flag: 100 + - azimuth_angle: 0.103308 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00626935 + elevation_angle_std: 0.0210671 + range: 202.068 + range_std: 0.200000 + range_rate: -6.15454 + range_rate_std: 0.250000 + rcs: 15 + measurement_id: 409 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 1 + object_id: 4287 + ambiguity_flag: 100 + - azimuth_angle: 0.0736255 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0131078 + elevation_angle_std: 0.0358991 + range: 204.112 + range_std: 0.200000 + range_rate: -6.16270 + range_rate_std: 0.250000 + rcs: 19 + measurement_id: 410 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 3 + object_id: 9365 + ambiguity_flag: 100 + - azimuth_angle: 0.100173 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0102271 + elevation_angle_std: 0.0308281 + range: 211.979 + range_std: 0.200000 + range_rate: -6.14264 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 411 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 7 + object_id: 717 + ambiguity_flag: 100 + - azimuth_angle: 0.0945462 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00632987 + elevation_angle_std: 0.0418043 + range: 212.144 + range_std: 0.200000 + range_rate: -6.38830 + range_rate_std: 0.250000 + rcs: 4 + measurement_id: 412 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 12 + object_id: 717 + ambiguity_flag: 0 + - azimuth_angle: 0.0611591 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.209109 + elevation_angle_std: 0.0731464 + range: 217.658 + range_std: 0.200000 + range_rate: -6.15577 + range_rate_std: 0.250000 + rcs: 19 + measurement_id: 413 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 8 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: 0.00975850 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.00929516 + elevation_angle_std: 0.0418043 + range: 217.857 + range_std: 0.200000 + range_rate: -6.16691 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 414 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 7273 + ambiguity_flag: 100 + - azimuth_angle: 0.0860432 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0290268 + elevation_angle_std: 0.0563254 + range: 226.169 + range_std: 0.200000 + range_rate: -6.15695 + range_rate_std: 0.250000 + rcs: 9 + measurement_id: 416 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 28 + object_id: 9650 + ambiguity_flag: 100 + - azimuth_angle: 0.100636 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0361713 + elevation_angle_std: 0.0502777 + range: 228.666 + range_std: 0.200000 + range_rate: -6.15429 + range_rate_std: 0.250000 + rcs: 21 + measurement_id: 417 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 8 + object_id: 9650 + ambiguity_flag: 100 + - azimuth_angle: 0.101214 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0210606 + elevation_angle_std: 0.0631005 + range: 230.032 + range_std: 0.200000 + range_rate: -6.13939 + range_rate_std: 0.250000 + rcs: 5 + measurement_id: 418 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 7 + object_id: 9650 + ambiguity_flag: 70 + - azimuth_angle: -0.0502327 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0904714 + elevation_angle_std: 0.0731464 + range: 232.235 + range_std: 0.200000 + range_rate: -6.12117 + range_rate_std: 0.250000 + rcs: 20 + measurement_id: 419 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 6 + object_id: 20433 + ambiguity_flag: 100 + - azimuth_angle: -0.0508036 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.184205 + elevation_angle_std: 0.0706906 + range: 235.015 + range_std: 0.200000 + range_rate: -6.03608 + range_rate_std: 0.250000 + rcs: 21 + measurement_id: 421 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 7 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.112366 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.108047 + elevation_angle_std: 0.0731464 + range: 235.513 + range_std: 0.200000 + range_rate: -6.06316 + range_rate_std: 0.250000 + rcs: 14 + measurement_id: 423 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 1600 + ambiguity_flag: 100 + - azimuth_angle: -0.109372 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.130757 + elevation_angle_std: 0.0448793 + range: 236.284 + range_std: 0.200000 + range_rate: -6.05573 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 424 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 11 + object_id: 1600 + ambiguity_flag: 100 + - azimuth_angle: 0.0964568 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00331622 + elevation_angle_std: 0.0143966 + range: 238.492 + range_std: 0.200000 + range_rate: -6.15025 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 428 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 5 + object_id: 4883 + ambiguity_flag: 100 + - azimuth_angle: -0.113015 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.191819 + elevation_angle_std: 0.0332671 + range: 238.736 + range_std: 0.200000 + range_rate: -5.98308 + range_rate_std: 0.250000 + rcs: 19 + measurement_id: 427 + positive_predictive_value: 100 + classification: 0 + multi_target_probability: 5 + object_id: 0 + ambiguity_flag: 0 + - azimuth_angle: -0.0422297 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.107621 + elevation_angle_std: 0.0210671 + range: 241.045 + range_std: 0.200000 + range_rate: -6.10944 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 429 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 4 + object_id: 5484 + ambiguity_flag: 100 + - azimuth_angle: -0.0363042 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0675977 + elevation_angle_std: 0.0387393 + range: 241.544 + range_std: 0.200000 + range_rate: -6.14165 + range_rate_std: 0.250000 + rcs: 14 + measurement_id: 430 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 0 + object_id: 5484 + ambiguity_flag: 100 + - azimuth_angle: -0.0363192 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.140089 + elevation_angle_std: 0.0731464 + range: 243.748 + range_std: 0.200000 + range_rate: -6.06375 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 432 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 18 + object_id: 5484 + ambiguity_flag: 50 + - azimuth_angle: -0.0324828 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0822471 + elevation_angle_std: 0.0731464 + range: 243.788 + range_std: 0.200000 + range_rate: -6.15908 + range_rate_std: 0.250000 + rcs: 11 + measurement_id: 433 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 5484 + ambiguity_flag: 50 + - azimuth_angle: -0.0364788 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.117487 + elevation_angle_std: 0.0731464 + range: 244.315 + range_std: 0.200000 + range_rate: -6.15999 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 434 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 27 + object_id: 5484 + ambiguity_flag: 0 + - azimuth_angle: -0.0313597 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0928658 + elevation_angle_std: 0.0358991 + range: 245.657 + range_std: 0.200000 + range_rate: -6.13414 + range_rate_std: 0.250000 + rcs: 16 + measurement_id: 435 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 2 + object_id: 5484 + ambiguity_flag: 100 + - azimuth_angle: -0.0332655 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.133406 + elevation_angle_std: 0.0731464 + range: 246.606 + range_std: 0.200000 + range_rate: -6.11262 + range_rate_std: 0.250000 + rcs: 14 + measurement_id: 436 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 40 + object_id: 5484 + ambiguity_flag: 100 + - azimuth_angle: -0.0320791 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0885150 + elevation_angle_std: 0.0563254 + range: 247.051 + range_std: 0.200000 + range_rate: -6.15159 + range_rate_std: 0.250000 + rcs: 13 + measurement_id: 437 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 5484 + ambiguity_flag: 100 + - azimuth_angle: 0.0942807 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00282000 + elevation_angle_std: 0.0706906 + range: 252.742 + range_std: 0.200000 + range_rate: -6.15310 + range_rate_std: 0.250000 + rcs: 15 + measurement_id: 438 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 3 + object_id: 422 + ambiguity_flag: 100 + - azimuth_angle: 0.0927978 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.0206853 + elevation_angle_std: 0.0563254 + range: 259.601 + range_std: 0.200000 + range_rate: -6.14625 + range_rate_std: 0.250000 + rcs: 10 + measurement_id: 439 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 16 + object_id: 423 + ambiguity_flag: 100 + - azimuth_angle: 0.0924008 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00000 + elevation_angle_std: 0.0731464 + range: 260.267 + range_std: 0.200000 + range_rate: -6.15265 + range_rate_std: 0.250000 + rcs: 16 + measurement_id: 440 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 1 + object_id: 423 + ambiguity_flag: 100 + - azimuth_angle: 0.0990405 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: -0.00383665 + elevation_angle_std: 0.0143966 + range: 281.245 + range_std: 0.200000 + range_rate: -6.15023 + range_rate_std: 0.250000 + rcs: 18 + measurement_id: 441 + positive_predictive_value: 100 + classification: 4 + multi_target_probability: 10 + object_id: 424 + ambiguity_flag: 100 + - azimuth_angle: 0.0113207 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0442989 + elevation_angle_std: 0.0502777 + range: 283.044 + range_std: 0.200000 + range_rate: -6.15523 + range_rate_std: 0.250000 + rcs: 7 + measurement_id: 442 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 22 + object_id: 431 + ambiguity_flag: 100 + - azimuth_angle: 0.0506422 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0435963 + elevation_angle_std: 0.0448793 + range: 285.821 + range_std: 0.200000 + range_rate: -6.17617 + range_rate_std: 0.250000 + rcs: 12 + measurement_id: 443 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 29 + object_id: 712 + ambiguity_flag: 100 + - azimuth_angle: 0.0508674 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0399138 + elevation_angle_std: 0.0731464 + range: 285.953 + range_std: 0.200000 + range_rate: -6.17227 + range_rate_std: 0.250000 + rcs: 14 + measurement_id: 444 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 13 + object_id: 712 + ambiguity_flag: 100 + - azimuth_angle: 0.0476959 + azimuth_angle_std: 0.00872663 + invalid_distance: false + invalid_distance_std: false + invalid_azimuth: false + invalid_azimuth_std: false + invalid_elevation: false + invalid_elevation_std: false + invalid_range_rate: false + invalid_range_rate_std: false + elevation_angle: 0.0403862 + elevation_angle_std: 0.0631005 + range: 292.677 + range_std: 0.200000 + range_rate: -6.17718 + range_rate_std: 0.250000 + rcs: 9 + measurement_id: 445 + positive_predictive_value: 100 + classification: 3 + multi_target_probability: 12 + object_id: 9348 + ambiguity_flag: 100 diff --git a/nebula_tests/package.xml b/nebula_tests/package.xml index 4b79ad749..57593c96f 100644 --- a/nebula_tests/package.xml +++ b/nebula_tests/package.xml @@ -12,12 +12,14 @@ ament_cmake_auto ros_environment + continental_msgs diagnostic_msgs diagnostic_updater libpcl-all-dev nebula_common nebula_decoders nebula_hw_interfaces + nebula_msgs nebula_ros pcl_conversions rclcpp From a08edbdd2602e9f9889bf0d1b22a5a3b6a30bf06 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 8 Mar 2024 18:18:42 +0900 Subject: [PATCH 32/42] chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp index d9726a821..b42948426 100644 --- a/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp +++ b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp @@ -42,7 +42,7 @@ class NebulaHwInterfaceWrapperBase virtual Status InitializeHwInterface( const drivers::SensorConfigurationBase & sensor_configuration) = 0; // void SendDataPacket(const std::vector &buffer); // Ideally this will be - // implemented as specific funtions, GetFanStatus, GetEchoMode + // implemented as specific functions, GetFanStatus, GetEchoMode /// @brief Enable sensor setup during initialization and set_parameters_callback bool setup_sensor; From 6e1a8abc60f067fd64b571e9a92c5cacb7f75f2d Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Tue, 12 Mar 2024 18:58:39 +0900 Subject: [PATCH 33/42] fix: reflected the change of a status message in checks and changed the qos of a topic Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.cpp | 4 ++-- .../continental_ars548_hw_interface_ros_wrapper.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 d1a8cff34..a86687afb 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 @@ -146,7 +146,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( msg.detections.resize(number_of_detections); // Estimate dropped detections only when the radar is synchronized - if (radar_status_.timestamp_sync_status == "SYNC_OK") { + if (radar_status_.timestamp_sync_status == "1:SYNC_OK") { if (radar_status_.detection_first_stamp == 0) { radar_status_.detection_first_stamp = static_cast(msg.header.stamp.sec) * 1'000'000'000 + @@ -255,7 +255,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( msg.objects.resize(number_of_objects); // Estimate dropped objects only when the radar is synchronized - if (radar_status_.timestamp_sync_status == "SYNC_OK") { + if (radar_status_.timestamp_sync_status == "1:SYNC_OK") { if (radar_status_.object_first_stamp == 0) { radar_status_.object_first_stamp = static_cast(msg.header.stamp.sec) * 1'000'000'000 + diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 164cc7bfb..bad08ad93 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -79,7 +79,7 @@ Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() std::placeholders::_1)); steering_angle_sub_ = create_subscription( - "steering_angle_input", rclcpp::QoS{1}, + "steering_angle_input", rclcpp::SensorDataQoS(), std::bind( &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1)); From d5194d73d86cfb5d7e05db483a3b6622d1a84e3b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Mar 2024 19:23:48 +0900 Subject: [PATCH 34/42] feat: there are cases (probably involving the mounting yaw) in which the objects are published in the sensor frame rather than in the base, so I added a new parameter Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 2 ++ .../decoders/continental_ars548_decoder.cpp | 2 +- nebula_ros/launch/continental_launch_all_hw.xml | 5 +++++ .../continental_ars548_decoder_ros_wrapper.cpp | 13 +++++++++++-- ...ontinental_ars548_hw_interface_ros_wrapper.cpp | 11 +++++++++++ ...ontinental_ars548_hw_interface_ros_wrapper.cpp | 11 +++++++++++ .../continental_ros_decoder_test_ars548.cpp | 15 ++++++++++++--- .../ars548/1708578204_199326753_object.yaml | 2 +- .../ars548/1708578204_202447652_detection.yaml | 2 +- 9 files changed, 55 insertions(+), 8 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 5dd3b1a11..b56fd03c5 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -47,6 +47,7 @@ struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase { std::string multicast_ip{}; std::string base_frame{}; + std::string object_frame{}; uint16_t configuration_host_port{}; uint16_t configuration_sensor_port{}; bool use_sensor_time{}; @@ -72,6 +73,7 @@ inline std::ostream & operator<<( std::ostream & os, ContinentalARS548SensorConfiguration const & arg) { os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip + << ", BaseFrame: " << arg.base_frame << ", ObjectFrame: " << arg.object_frame << ", ConfigurationHostPort: " << arg.configuration_host_port << ", ConfigurationSensorPort: " << arg.configuration_sensor_port << ", UseSensorTime: " << arg.use_sensor_time 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 a86687afb..6c28038b9 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 @@ -238,7 +238,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( std::memcpy(&object_list, data.data(), sizeof(object_list)); - msg.header.frame_id = sensor_configuration_->base_frame; + msg.header.frame_id = sensor_configuration_->object_frame; if (sensor_configuration_->use_sensor_time) { msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index a8660007a..2656c7d40 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -3,6 +3,7 @@ + @@ -41,6 +42,7 @@ + @@ -63,6 +65,7 @@ + @@ -86,6 +89,7 @@ + @@ -108,6 +112,7 @@ + diff --git a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp index 04b20f1fa..ecad7752f 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -161,6 +161,15 @@ Status ContinentalARS548DriverRosWrapper::GetParameters( this->declare_parameter("base_frame", descriptor); sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; @@ -683,7 +692,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT current_ids.emplace(object.object_id); visualization_msgs::msg::Marker box_marker; - box_marker.header.frame_id = sensor_cfg_ptr_->base_frame; + box_marker.header.frame_id = sensor_cfg_ptr_->object_frame; box_marker.header.stamp = msg.header.stamp; box_marker.ns = "boxes"; box_marker.id = object.object_id; @@ -764,7 +773,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DriverRosWrapper::ConvertT } visualization_msgs::msg::Marker delete_marker; - delete_marker.header.frame_id = sensor_cfg_ptr_->base_frame; + delete_marker.header.frame_id = sensor_cfg_ptr_->object_frame; delete_marker.header.stamp = msg.header.stamp; delete_marker.ns = "boxes"; delete_marker.id = previous_id; diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index bad08ad93..4dd5d765a 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -193,6 +193,15 @@ Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("base_frame", descriptor); sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -306,6 +315,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper: get_param(p, "data_port", new_param.data_port) | get_param(p, "multicast_ip", new_param.multicast_ip) | get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | @@ -479,6 +489,7 @@ ContinentalARS548HwInterfaceRosWrapper::updateParameters() rclcpp::Parameter("data_port", sensor_configuration_.data_port), rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), rclcpp::Parameter( "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), diff --git a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp index 58db41634..b8af00fd2 100644 --- a/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp @@ -173,6 +173,15 @@ Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( this->declare_parameter("base_frame", descriptor); sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; @@ -247,6 +256,7 @@ rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWra get_param(p, "data_port", new_param.data_port) | get_param(p, "multicast_ip", new_param.multicast_ip) | get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | get_param(p, "configuration_host_port", new_param.configuration_host_port) | @@ -333,6 +343,7 @@ MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() rclcpp::Parameter("data_port", sensor_configuration_.data_port), rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), rclcpp::Parameter( "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index 676aa2bd3..d31398c5b 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -104,7 +104,16 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("base_frame", "some_base_frame", descriptor); - sensor_configuration.frame_id = this->get_parameter("base_frame").as_string(); + 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; @@ -113,7 +122,7 @@ Status ContinentalRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("frame_id", "some_sensor_frame", descriptor); - sensor_configuration.base_frame = this->get_parameter("frame_id").as_string(); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -216,7 +225,7 @@ void ContinentalRosDecoderTest::DetectionListCallback( void ContinentalRosDecoderTest::ObjectListCallback( std::unique_ptr msg) { - EXPECT_EQ(sensor_cfg_ptr_->base_frame, msg->header.frame_id); + EXPECT_EQ(sensor_cfg_ptr_->object_frame, msg->header.frame_id); std::string msg_as_string = continental_msgs::msg::to_yaml(*msg); std::stringstream detection_path; diff --git a/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml b/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml index d64b06d91..68d13d2dc 100644 --- a/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml +++ b/nebula_tests/data/continental/ars548/1708578204_199326753_object.yaml @@ -2,7 +2,7 @@ header: stamp: sec: 1708578204 nanosec: 199326753 - frame_id: some_sensor_frame + frame_id: some_object_frame stamp_sync_status: 1 objects: - object_id: 21326 diff --git a/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml b/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml index 6268cc44a..2b65eacfc 100644 --- a/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml +++ b/nebula_tests/data/continental/ars548/1708578204_202447652_detection.yaml @@ -2,7 +2,7 @@ header: stamp: sec: 1708578204 nanosec: 202447652 - frame_id: some_base_frame + frame_id: some_sensor_frame stamp_sync_status: 1 origin_pos: x: 3.87200 From d813f4ae2722f47b26b42ea7ef954e9b112fd1af Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Mar 2024 21:17:49 +0900 Subject: [PATCH 35/42] feat: generalized launch script to support ars548 and updated the readme Signed-off-by: Kenzo Lobos-Tsunekawa --- README.md | 55 +++++++++---------- nebula_ros/config/continental/ARS548.yaml | 17 ++++++ nebula_ros/config/continental/SRR520.yaml | 14 +++++ .../launch/continental_launch_all_hw.xml | 11 ++-- nebula_ros/launch/nebula_launch.py | 31 ++++++++--- 5 files changed, 86 insertions(+), 42 deletions(-) create mode 100644 nebula_ros/config/continental/ARS548.yaml create mode 100644 nebula_ros/config/continental/SRR520.yaml diff --git a/README.md b/README.md index 933fbd024..a509950ca 100644 --- a/README.md +++ b/README.md @@ -3,6 +3,7 @@ Nebula is a sensor driver platform that is designed to provide a unified framework for as wide a variety of devices as possible. While it primarily targets Ethernet-based LiDAR sensors, it aims to be easily extendable to support new sensors and interfaces. Nebula provides the following features: + - Support for Velodyne and Hesai sensors, with other LiDAR vendor support under development - ROS 2 interface implementations - TCP/IP and UDP communication implementations @@ -14,10 +15,8 @@ Nebula provides the following features: - Receiving and interpretation of diagnostics information from the sensor - Support for multiple return modes and labelling of return types for each point - With a rapidly increasing number of sensor types and models becoming available, and varying levels of vendor and third-party driver support, Nebula creates a centralized driver methodology. We hope that this project will be used to facilitate active collaboration and efficiency in development projects by providing a platform that reduces the need to re-implement and maintain many different sensor drivers. Contributions to extend the supported devices and features of Nebula are always welcome. - ## How to build Nebula builds on ROS Galactic and Humble. @@ -85,27 +84,27 @@ ros2 launch nebula_ros nebula_launch.py sensor_model:=Pandar64 config_file:=your Supported models, where sensor_model is the ROS param to be used at launch: -| Manufacturer | Model | sensor_model | Configuration file | Test status | -| ------------ | ------------- | ------------ | ------------------ | ----------- | -| HESAI | Pandar 64 | Pandar64 | Pandar64.yaml | :heavy_check_mark: | -| HESAI | Pandar 40P | Pandar40P | Pandar40P.yaml | :heavy_check_mark: | -| HESAI | Pandar XT32 | PandarXT32 | PandarXT32.yaml | :heavy_check_mark: | -| HESAI | Pandar XT32M | PandarXT32M | PandarXT32M.yaml | :warning: | -| HESAI | Pandar QT64 | PandarQT64 | PandarQT64.yaml | :heavy_check_mark: | -| HESAI | Pandar QT128 | PandarQT128 | PandarQT128.yaml | :warning: | -| HESAI | Pandar AT128 | PandarAT128 | PandarAT128.yaml | :heavy_check_mark:* | -| HESAI | Pandar 128E4X | Pandar128E4X | Pandar128E4X.yaml | :warning: | -| Velodyne | VLP-16 | VLP16 | VLP16.yaml | :warning: | -| Velodyne | VLP-16-HiRes | VLP16 | | :x: | -| Velodyne | VLP-32 | VLP32 | VLP32.yaml | :warning: | -| Velodyne | VLS-128 | VLS128 | VLS128.yaml | :warning: | +| Manufacturer | Model | sensor_model | Configuration file | Test status | +| ------------ | ------------- | ------------ | ------------------ | -------------------- | +| HESAI | Pandar 64 | Pandar64 | Pandar64.yaml | :heavy_check_mark: | +| HESAI | Pandar 40P | Pandar40P | Pandar40P.yaml | :heavy_check_mark: | +| HESAI | Pandar XT32 | PandarXT32 | PandarXT32.yaml | :heavy_check_mark: | +| HESAI | Pandar XT32M | PandarXT32M | PandarXT32M.yaml | :warning: | +| HESAI | Pandar QT64 | PandarQT64 | PandarQT64.yaml | :heavy_check_mark: | +| HESAI | Pandar QT128 | PandarQT128 | PandarQT128.yaml | :warning: | +| HESAI | Pandar AT128 | PandarAT128 | PandarAT128.yaml | :heavy_check_mark:\* | +| HESAI | Pandar 128E4X | Pandar128E4X | Pandar128E4X.yaml | :warning: | +| Velodyne | VLP-16 | VLP16 | VLP16.yaml | :warning: | +| Velodyne | VLP-16-HiRes | VLP16 | | :x: | +| Velodyne | VLP-32 | VLP32 | VLP32.yaml | :warning: | +| Velodyne | VLS-128 | VLS128 | VLS128.yaml | :warning: | +| Continental | ARS548 | ARS548 | ARS548.yaml | :warning: | Test status:\ :heavy_check_mark:: complete\ :warning:: some functionality yet to be tested\ :x:: untested\ -*: AT128 needs software version 3.50.8 or newer for the `scan_angle` setting to work correctly. - +\*: AT128 needs software version 3.50.8 or newer for the `scan_angle` setting to work correctly. ## ROS parameters @@ -215,27 +214,26 @@ Parameters shared by all supported models: #### Driver parameters -| Parameter | Type | Default | Accepted values | Description | -| ---------------- | ------ | -------- | -------------------- | --------------------------------------- | -| frame_id | string | velodyne | | ROS frame ID | -| calibration_file | string | | | LiDAR calibration file | -| min_range | double | 0.3 | meters, >= 0.3 | Minimum point range published | -| max_range | double | 300.0 | meters, <= 300.0 | Maximum point range published | -| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | +| Parameter | Type | Default | Accepted values | Description | +| ---------------- | ------ | -------- | ---------------- | ----------------------------- | +| frame_id | string | velodyne | | ROS frame ID | +| calibration_file | string | | | LiDAR calibration file | +| min_range | double | 0.3 | meters, >= 0.3 | Minimum point range published | +| max_range | double | 300.0 | meters, <= 300.0 | Maximum point range published | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | ## Software design overview ![DriverOrganization](docs/diagram.png) - ## Hesai Sensor Setup New Hesai sensors do not provide a Web UI to verify and set up the sensor parameters. Instead, these offer a TCP-based protocol to obtain and set the configuration. Nebula sets these sensors at launch. However, settings such as Destination IP, Sensor IP, IP Mask, and Data Port might cause undesired sensor functioning if the driver gets mistakenly launched with inappropriate values. To overcome this problem, Nebula provides an additional setup script for these settings to avoid such scenarios. The script requires the installation of dependencies via pip: -`$ pip3 install scripts/requirements.txt # first-time setup` +`$ pip3 install scripts/requirements.txt # first-time setup` Once the dependencies are installed, the setup script can be invoked using the following command: @@ -274,6 +272,7 @@ Run profiling for each version you want to compare: git checkout my_improved_branch ./scripts/profiling_runner.bash improved -m Pandar64 -b ~/my_rosbag -c 2 -t 20 -n 3 ``` + Show results: ```bash diff --git a/nebula_ros/config/continental/ARS548.yaml b/nebula_ros/config/continental/ARS548.yaml new file mode 100644 index 000000000..2342cfe93 --- /dev/null +++ b/nebula_ros/config/continental/ARS548.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + sensor_model: "ARS548" + frame_id: "continental" + base_frame: "base_link" + object_frame: "base_link" + sensor_ip: "10.13.1.114" + host_ip: "10.13.1.166" + multicast_ip: "224.0.2.2" + data_port: 42102 + configuration_host_port: 42401 + configuration_sensor_port: 42101 + use_sensor_time: false + configuration_vehicle_length: 4.89 + configuration_vehicle_width: 1.896 + configuration_vehicle_height: 2.5 + configuration_vehicle_wheelbase: 2.79 diff --git a/nebula_ros/config/continental/SRR520.yaml b/nebula_ros/config/continental/SRR520.yaml new file mode 100644 index 000000000..5b04570a5 --- /dev/null +++ b/nebula_ros/config/continental/SRR520.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + sensor_model: "SRR520" + frame_id: "continental" + base_frame: base_link + interface: "can4" + receiver_timeout_sec: 0.03 + sender_timeout_sec: 0.01 + filters: "0:0" + use_bus_time: false + new_sensor_id: 0 + new_plug_bottom: true + new_cover_damping: 0.0 + reset_sensor_configuration: false diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index 2656c7d40..dd5f270a8 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -1,5 +1,6 @@ + @@ -11,7 +12,6 @@ - @@ -45,7 +45,6 @@ - @@ -56,7 +55,7 @@ + name="continental_ars548_hw_driver" output="screen" if="$(var launch_hw)"> @@ -70,7 +69,6 @@ - @@ -92,7 +90,6 @@ - @@ -103,7 +100,7 @@ + name="multi_continental_ars548_hw_driver" output="screen" if="$(var launch_hw)"> @@ -148,7 +145,7 @@ + name="continental_srr520_hw_driver" output="screen" if="$(var launch_hw)"> diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f22a9d6c3..132b19e5b 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -15,22 +15,34 @@ import yaml -def get_lidar_make(sensor_name): +def get_sensor_make(sensor_name): if sensor_name[:6].lower() == "pandar": return "Hesai", ".csv" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: return "Velodyne", ".yaml" elif sensor_name.lower() in ["helios", "bpearl"]: return "Robosense", None - return "unrecognized_sensor_model" + elif sensor_name.lower() == "ars548": + return "Continental", None + return "unrecognized_sensor_model", None +def get_plugin_name(sensor_make, sensor_model): + if sensor_make.lower() != "continental": + return sensor_make + elif sensor_model.lower() == "ars548": + return "ContinentalARS548" + else: + return "invalid_plugin" + +def is_hw_monitor_available(sensor_make): + return sensor_make.lower() != "continental" def launch_setup(context, *args, **kwargs): # Model and make sensor_model = LaunchConfiguration("sensor_model").perform(context) calibration_file = LaunchConfiguration("calibration_file").perform(context) correction_file = LaunchConfiguration("correction_file").perform(context) - sensor_make, sensor_extension = get_lidar_make(sensor_model) + sensor_make, sensor_extension = get_sensor_make(sensor_model) nebula_decoders_share_dir = get_package_share_directory("nebula_decoders") nebula_ros_share_dir = get_package_share_directory("nebula_ros") @@ -53,12 +65,14 @@ def launch_setup(context, *args, **kwargs): with open(sensor_params_fp, "r") as f: sensor_params = yaml.safe_load(f)["/**"]["ros__parameters"] nodes = [] - if LaunchConfiguration("launch_hw").perform(context) == "true": + launch_hw = LaunchConfiguration("launch_hw").perform(context) == "true" + + if launch_hw: nodes.append( # HwInterface ComposableNode( package="nebula_ros", - plugin=sensor_make+"HwInterfaceRosWrapper", + plugin=get_plugin_name(sensor_make, sensor_model)+"HwInterfaceRosWrapper", name=sensor_make.lower()+"_hw_interface_ros_wrapper_node", parameters=[ sensor_params, @@ -76,6 +90,9 @@ def launch_setup(context, *args, **kwargs): ], ), ) + + + if launch_hw and is_hw_monitor_available(sensor_make): nodes.append( # HwMonitor ComposableNode( @@ -97,7 +114,7 @@ def launch_setup(context, *args, **kwargs): nodes.append( ComposableNode( package="nebula_ros", - plugin=sensor_make+"DriverRosWrapper", + plugin=get_plugin_name(sensor_make, sensor_model)+"DriverRosWrapper", name=sensor_make.lower()+"_driver_ros_wrapper_node", parameters=[ sensor_params, @@ -125,7 +142,7 @@ def launch_setup(context, *args, **kwargs): container_kwargs = {} if LaunchConfiguration("debug_logging").perform(context) == "true": container_kwargs["ros_arguments"] = ['--log-level', 'debug'] - + container = ComposableNodeContainer( name="nebula_ros_node", namespace="", From d4e2688046808e8be48b8373727d795349742a0e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Thu, 14 Mar 2024 22:26:43 +0900 Subject: [PATCH 36/42] fix: merge error Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index 837696157..a5e5bd068 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -274,7 +274,7 @@ void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector & } } } -Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } +Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } From 851dd860f9b8f921155d996fdc30009812525c11 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 15 Mar 2024 17:48:40 +0900 Subject: [PATCH 37/42] chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 6c28038b9..8e1bbed05 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 @@ -556,10 +556,10 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( temperature_status_vector.push_back("Ok"); } if (sensor_status_packet.temperature_status & 0x01) { - temperature_status_vector.push_back("Current under temperature"); + temperature_status_vector.push_back("Current undertemperature"); } if (sensor_status_packet.temperature_status & 0x02) { - temperature_status_vector.push_back("Past under temperature"); + temperature_status_vector.push_back("Past undertemperature"); } if (sensor_status_packet.temperature_status & 0x04) { temperature_status_vector.push_back("Current over temperature"); From aeaad506b9534924d132c54ccd2676ec069f2454 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 15 Mar 2024 17:53:40 +0900 Subject: [PATCH 38/42] chore: fixed another spelling Signed-off-by: Kenzo Lobos-Tsunekawa --- .../decoders/continental_ars548_decoder.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 8e1bbed05..fb2a44f60 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 @@ -562,10 +562,10 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( temperature_status_vector.push_back("Past undertemperature"); } if (sensor_status_packet.temperature_status & 0x04) { - temperature_status_vector.push_back("Current over temperature"); + temperature_status_vector.push_back("Current overtemperature"); } if (sensor_status_packet.temperature_status & 0x08) { - temperature_status_vector.push_back("Past over temperature"); + temperature_status_vector.push_back("Past overtemperature"); } radar_status_.voltage_status = boost::algorithm::join(voltage_status_vector, ", "); From 764e584d1e9caa722e73307123f48223a51e265a Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Fri, 15 Mar 2024 18:02:09 +0900 Subject: [PATCH 39/42] chore: yet another spell fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../multi_continental_ars548_hw_interface_ros_wrapper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp index 1edf7bc04..19429b99c 100644 --- a/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -63,7 +63,7 @@ bool get_param(const std::vector & p, const std::string & nam } /// @brief Hardware interface ros wrapper of continental radar ethernet driver -/// NOTE: this is a temporal class that acts as a single hw interface for multiple devices +/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices /// The reason behind this is a not-so-efficient multicasting and package processing when having N /// interfaces for N devices If we end up having problems because of that we may switch to this /// implementation. Otherwise this implementation will be removed at a later date From 100757164b9eee0ced387ed2c5276687796abbfd Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 18 Mar 2024 10:09:40 +0900 Subject: [PATCH 40/42] chore: deleted SRR files that leaked into the ARS branch Signed-off-by: Kenzo Lobos-Tsunekawa --- nebula_ros/config/continental/SRR520.yaml | 14 ----- .../launch/continental_launch_all_hw.xml | 62 ------------------- 2 files changed, 76 deletions(-) delete mode 100644 nebula_ros/config/continental/SRR520.yaml diff --git a/nebula_ros/config/continental/SRR520.yaml b/nebula_ros/config/continental/SRR520.yaml deleted file mode 100644 index 5b04570a5..000000000 --- a/nebula_ros/config/continental/SRR520.yaml +++ /dev/null @@ -1,14 +0,0 @@ -/**: - ros__parameters: - sensor_model: "SRR520" - frame_id: "continental" - base_frame: base_link - interface: "can4" - receiver_timeout_sec: 0.03 - sender_timeout_sec: 0.01 - filters: "0:0" - use_bus_time: false - new_sensor_id: 0 - new_plug_bottom: true - new_cover_damping: 0.0 - reset_sensor_configuration: false diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml index dd5f270a8..d5ed36078 100644 --- a/nebula_ros/launch/continental_launch_all_hw.xml +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -24,18 +24,6 @@ - - - - - - - - - - - - @@ -121,54 +109,4 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 423112d69d8bfec2446adae90cf4ce20dae5c505 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 18 Mar 2024 11:36:42 +0900 Subject: [PATCH 41/42] feat: added an option to set all the parameters directly via services Signed-off-by: Kenzo Lobos-Tsunekawa --- .../ContinentalArs548SetSensorMounting.srv | 6 ++ .../ContinentalArs548SetVehicleParameters.srv | 4 + ...nental_ars548_hw_interface_ros_wrapper.cpp | 99 ++++++++++++++----- 3 files changed, 85 insertions(+), 24 deletions(-) diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv index 8b85557ea..32a8d60f2 100644 --- a/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv @@ -1,3 +1,9 @@ +bool autoconfigure_extrinsics 1 +float32 longitudinal +float32 lateral +float32 vertical +float32 yaw +float32 pitch bool plug_orientation # 0 = PLUG_RIGHT. 1 = PLUG_LEFT --- bool success diff --git a/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv b/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv index d31c8da81..72ce8025c 100644 --- a/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv @@ -1,3 +1,7 @@ +float32 vehicle_length -1.0 +float32 vehicle_width -1.0 +float32 vehicle_height -1.0 +float32 vehicle_wheelbase -1.0 --- bool success string message # status messages diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 4dd5d765a..d7baa3f6a 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -412,29 +412,46 @@ void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( auto tf_buffer = std::make_unique(this->get_clock()); auto tf_listener = std::make_unique(*tf_buffer); - geometry_msgs::msg::TransformStamped base_to_sensor_tf; - try { - base_to_sensor_tf = tf_buffer->lookupTransform( - sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), - rclcpp::Duration::from_seconds(0.5)); - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", - sensor_configuration_.frame_id.c_str(), ex.what()); - response->success = false; - response->message = ex.what(); - return; + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + this->get_logger(), + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", + sensor_configuration_.frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - + sensor_configuration_.configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; } - 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); - auto result = hw_interface_.SetSensorMounting( - base_to_sensor_tf.transform.translation.x - - sensor_configuration_.configuration_vehicle_wheelbase, - base_to_sensor_tf.transform.translation.y, base_to_sensor_tf.transform.translation.z, rpy.z, - rpy.y, request->plug_orientation); + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); response->success = result == Status::OK; response->message = (std::stringstream() << result).str(); @@ -447,12 +464,46 @@ void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback const std::shared_ptr response) { + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_length); + vehicle_length = sensor_configuration_.configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_width); + vehicle_width = sensor_configuration_.configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_height); + vehicle_height = sensor_configuration_.configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_wheelbase); + vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; + } + std::scoped_lock lock(mtx_config_); auto result = hw_interface_.SetVehicleParameters( - sensor_configuration_.configuration_vehicle_length, - sensor_configuration_.configuration_vehicle_width, - sensor_configuration_.configuration_vehicle_height, - sensor_configuration_.configuration_vehicle_wheelbase); + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); response->success = result == Status::OK; response->message = (std::stringstream() << result).str(); From fb1b08bcd37d0294e60675cfc0ec78f4349a78c4 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 18 Mar 2024 11:41:58 +0900 Subject: [PATCH 42/42] chore: updated cspell dict Signed-off-by: Kenzo Lobos-Tsunekawa --- .cspell.json | 1 + 1 file changed, 1 insertion(+) diff --git a/.cspell.json b/.cspell.json index db5683e72..4f17f7ddd 100644 --- a/.cspell.json +++ b/.cspell.json @@ -7,6 +7,7 @@ "Adctp", "AT", "block_id", + "extrinsics", "gprmc", "Hesai", "memcpy",