From cb3c56a1c81fe567f8fa44c8f55b5d354a8c0d2f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Thu, 28 Mar 2024 17:48:18 +0900 Subject: [PATCH] feat(ars548): initial driver (#123) * 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 * 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 * fix: fixed errors with naming and symbols after the refactoring Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed spell misses Signed-off-by: Kenzo Lobos-Tsunekawa * chore: more spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa * fix: updated the build_repos to point to the custom transport drivers for this PR Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed duplicated message file Signed-off-by: Kenzo Lobos-Tsunekawa * Update nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> * chore: addressed comments from the review related con consitency and unused code Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed default parameters in line with the awf guidelines Signed-off-by: Kenzo Lobos-Tsunekawa * chore: replaced the confusing position orientation name by simply otientation Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed unused `using` Signed-off-by: Kenzo Lobos-Tsunekawa * feat: implemented boost endian approach Also fixed some typos Both implementations provide the same results Signed-off-by: Kenzo Lobos-Tsunekawa * chore: removed the old implementation Signed-off-by: Kenzo Lobos-Tsunekawa * chore: mixed misspells Signed-off-by: Kenzo Lobos-Tsunekawa * fix: fixed short circuit in conditional statement, missing lock, and typo Signed-off-by: Kenzo Lobos-Tsunekawa * 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 * feat: added the missing steerin angle input Signed-off-by: Kenzo Lobos-Tsunekawa * feat: missing features for proper vehicle integration. fixed fixes and added visualization markers Signed-off-by: Kenzo Lobos-Tsunekawa * fix: had implemented the reference point correction for all relevant interfaces but the radar track msgs Signed-off-by: Kenzo Lobos-Tsunekawa * 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 * chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa * fix: covariance matrix Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added documentation on the ros wrapper regarding the reference point Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added documentation regarding the dropped packages estimation Signed-off-by: Kenzo Lobos-Tsunekawa * fix: replaced if-elses by switches and fixed a status vatiable decoding Signed-off-by: Kenzo Lobos-Tsunekawa * chore: spell fixes Signed-off-by: Kenzo Lobos-Tsunekawa * feat: refactored parameter setting into custom services (as much as possible) Signed-off-by: Kenzo Lobos-Tsunekawa * chore: added documentation regarding the multi hw interface Signed-off-by: Kenzo Lobos-Tsunekawa * chore: reverted pre-commit related changes (it will not pass pre-commit now though) Signed-off-by: Kenzo Lobos-Tsunekawa * feat: added unit tests for the ars548 Signed-off-by: Kenzo Lobos-Tsunekawa * chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa * fix: reflected the change of a status message in checks and changed the qos of a topic Signed-off-by: Kenzo Lobos-Tsunekawa * 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 * feat: generalized launch script to support ars548 and updated the readme Signed-off-by: Kenzo Lobos-Tsunekawa * fix: merge error Signed-off-by: Kenzo Lobos-Tsunekawa * chore: spell fix Signed-off-by: Kenzo Lobos-Tsunekawa * chore: fixed another spelling Signed-off-by: Kenzo Lobos-Tsunekawa * chore: yet another spell fix Signed-off-by: Kenzo Lobos-Tsunekawa * chore: deleted SRR files that leaked into the ARS branch Signed-off-by: Kenzo Lobos-Tsunekawa * feat: added an option to set all the parameters directly via services Signed-off-by: Kenzo Lobos-Tsunekawa * chore: updated cspell dict Signed-off-by: Kenzo Lobos-Tsunekawa --------- Signed-off-by: Kenzo Lobos-Tsunekawa Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> --- .cspell.json | 3 + README.md | 55 +- build_depends.repos | 4 +- .../continental/continental_ars548.hpp | 698 ++ .../nebula_common/hesai/hesai_common.hpp | 4 +- .../include/nebula_common/nebula_common.hpp | 48 +- .../robosense/robosense_common.hpp | 4 +- .../velodyne/velodyne_common.hpp | 4 +- nebula_decoders/CMakeLists.txt | 5 + .../decoders/continental_ars548_decoder.hpp | 104 + .../decoders/continental_packets_decoder.hpp | 49 + nebula_decoders/package.xml | 6 +- .../decoders/continental_ars548_decoder.cpp | 623 ++ nebula_hw_interfaces/CMakeLists.txt | 6 + .../nebula_hw_interface_base.hpp | 11 +- .../continental_ars548_hw_interface.hpp | 218 + .../multi_continental_ars548_hw_interface.hpp | 179 + .../hesai_hw_interface.hpp | 6 +- .../robosense_hw_interface.hpp | 6 +- .../velodyne_hw_interface.hpp | 6 +- nebula_hw_interfaces/package.xml | 7 +- .../continental_ars548_hw_interface.cpp | 625 ++ .../multi_continental_ars548_hw_interface.cpp | 453 ++ .../hesai_hw_interface.cpp | 8 +- .../robosense_hw_interface.cpp | 10 +- .../velodyne_hw_interface.cpp | 8 +- .../continental_msgs/CMakeLists.txt | 27 + .../msg/ContinentalArs548Detection.msg | 23 + .../msg/ContinentalArs548DetectionList.msg | 13 + .../msg/ContinentalArs548Object.msg | 44 + .../msg/ContinentalArs548ObjectList.msg | 3 + nebula_messages/continental_msgs/package.xml | 26 + .../continental_srvs/CMakeLists.txt | 26 + nebula_messages/continental_srvs/package.xml | 26 + ...ntinentalArs548SetNetworkConfiguration.srv | 4 + .../ContinentalArs548SetRadarParameters.srv | 9 + .../ContinentalArs548SetSensorMounting.srv | 10 + .../ContinentalArs548SetVehicleParameters.srv | 7 + 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 | 27 + nebula_ros/config/continental/ARS548.yaml | 17 + .../common/nebula_driver_ros_wrapper_base.hpp | 8 +- .../nebula_hw_interface_ros_wrapper_base.hpp | 4 +- ...continental_ars548_decoder_ros_wrapper.hpp | 166 + ...nental_ars548_hw_interface_ros_wrapper.hpp | 188 + ...nental_ars548_hw_interface_ros_wrapper.hpp | 146 + .../hesai/hesai_hw_interface_ros_wrapper.hpp | 2 +- .../robosense_decoder_ros_wrapper.hpp | 4 +- .../robosense_hw_interface_ros_wrapper.hpp | 2 +- .../velodyne_hw_interface_ros_wrapper.hpp | 2 +- .../launch/continental_launch_all_hw.xml | 112 + nebula_ros/launch/nebula_launch.py | 31 +- nebula_ros/package.xml | 11 +- ...continental_ars548_decoder_ros_wrapper.cpp | 802 ++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 561 ++ ...nental_ars548_hw_interface_ros_wrapper.cpp | 356 + .../hesai/hesai_hw_interface_ros_wrapper.cpp | 2 +- .../robosense_decoder_ros_wrapper.cpp | 2 +- .../robosense_hw_interface_ros_wrapper.cpp | 4 +- .../velodyne_hw_interface_ros_wrapper.cpp | 2 +- nebula_tests/CMakeLists.txt | 3 + nebula_tests/continental/CMakeLists.txt | 20 + .../continental_ros_decoder_test_ars548.cpp | 289 + .../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 + 73 files changed, 16626 insertions(+), 95 deletions(-) create mode 100644 nebula_common/include/nebula_common/continental/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_decoder.cpp create mode 100644 nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.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/continental_ars548_hw_interface.cpp create mode 100644 nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_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/package.xml 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 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/config/continental/ARS548.yaml create mode 100644 nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp create mode 100644 nebula_ros/launch/continental_launch_all_hw.xml create mode 100644 nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp create mode 100644 nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp 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/.cspell.json b/.cspell.json index dc00c35b3..4f17f7ddd 100644 --- a/.cspell.json +++ b/.cspell.json @@ -7,6 +7,7 @@ "Adctp", "AT", "block_id", + "extrinsics", "gprmc", "Hesai", "memcpy", @@ -39,6 +40,8 @@ "gptp", "Idat", "Vdat", + "autosar", + "srvs", "manc", "ipaddr", "ntoa" 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/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 diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp new file mode 100644 index 000000000..b56fd03c5 --- /dev/null +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -0,0 +1,698 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "boost/endian/buffers.hpp" +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ + +/// @brief struct for ARS548 sensor configuration +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{}; + float configuration_vehicle_length{}; + float configuration_vehicle_width{}; + float configuration_vehicle_height{}; + float configuration_vehicle_wheelbase{}; +}; + +/// @brief struct for Multiple ARS548 sensor configuration +struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration +{ + std::vector sensor_ips{}; + std::vector frame_ids{}; +}; + +/// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the << +/// operator) +/// @param os +/// @param arg +/// @return stream +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 + << ", ConfigurationVehicleLength: " << arg.configuration_vehicle_length + << ", ConfigurationVehicleWidth: " << arg.configuration_vehicle_width + << ", ConfigurationVehicleHeight: " << arg.configuration_vehicle_height + << ", ConfigurationVehicleWheelbase: " << arg.configuration_vehicle_wheelbase; + 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; +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; + +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 +{ + big_uint16_buf_t service_id{}; + big_uint16_buf_t method_id{}; + big_uint32_buf_t length{}; +}; + +struct HeaderSomeIPPacket +{ + big_uint16_buf_t client_id; + big_uint16_buf_t session_id; + uint8_t protocol_version; + uint8_t interface_version; + uint8_t message_type; + uint8_t return_code; +}; + +struct HeaderE2EP07Packet +{ + big_uint64_buf_t crc; + big_uint32_buf_t length; + big_uint32_buf_t sqc; + big_uint32_buf_t data_id; +}; + +struct StampSyncStatusPacket +{ + big_uint32_buf_t timestamp_nanoseconds; + big_uint32_buf_t timestamp_seconds; + uint8_t timestamp_sync_status; +}; + +struct DetectionPacket +{ + big_float32_buf_t azimuth_angle; + big_float32_buf_t azimuth_angle_std; + uint8_t invalid_flags; + big_float32_buf_t elevation_angle; + big_float32_buf_t elevation_angle_std; + big_float32_buf_t range; + big_float32_buf_t range_std; + big_float32_buf_t range_rate; + big_float32_buf_t range_rate_std; + int8_t rcs; + big_uint16_buf_t measurement_id; + uint8_t positive_predictive_value; + uint8_t classification; + uint8_t multi_target_probability; + big_uint16_buf_t object_id; + uint8_t ambiguity_flag; + big_uint16_buf_t sort_index; +}; + +struct DetectionListPacket +{ + HeaderPacket header; + HeaderSomeIPPacket header_some_ip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; + big_uint32_buf_t event_data_qualifier; + uint8_t extended_qualifier; + big_uint16_buf_t origin_invalid_flags; + big_float32_buf_t origin_x_pos; + big_float32_buf_t origin_x_std; + big_float32_buf_t origin_y_pos; + big_float32_buf_t origin_y_std; + big_float32_buf_t origin_z_pos; + big_float32_buf_t origin_z_std; + big_float32_buf_t origin_roll; + big_float32_buf_t origin_roll_std; + big_float32_buf_t origin_pitch; + big_float32_buf_t origin_pitch_std; + big_float32_buf_t origin_yaw; + big_float32_buf_t origin_yaw_std; + uint8_t list_invalid_flags; + DetectionPacket detections[MAX_DETECTIONS]; + big_float32_buf_t list_rad_vel_domain_min; + big_float32_buf_t list_rad_vel_domain_max; + big_uint32_buf_t number_of_detections; + big_float32_buf_t alignment_azimuth_correction; + big_float32_buf_t alignment_elevation_correction; + uint8_t alignment_status; + uint8_t reserved[14]; +}; + +struct ObjectPacket +{ + big_uint16_buf_t status_sensor; + big_uint32_buf_t id; + big_uint16_buf_t age; + uint8_t status_measurement; + uint8_t status_movement; + big_uint16_buf_t position_invalid_flags; + uint8_t position_reference; + big_float32_buf_t position_x; + big_float32_buf_t position_x_std; + big_float32_buf_t position_y; + big_float32_buf_t position_y_std; + big_float32_buf_t position_z; + big_float32_buf_t position_z_std; + big_float32_buf_t position_covariance_xy; + big_float32_buf_t position_orientation; + big_float32_buf_t position_orientation_std; + uint8_t existence_invalid_flags; + big_float32_buf_t existence_probability; + big_float32_buf_t existence_ppv; + uint8_t classification_car; + uint8_t classification_truck; + uint8_t classification_motorcycle; + uint8_t classification_bicycle; + uint8_t classification_pedestrian; + uint8_t classification_animal; + uint8_t classification_hazard; + uint8_t classification_unknown; + uint8_t classification_overdrivable; + uint8_t classification_underdrivable; + uint8_t dynamics_abs_vel_invalid_flags; + big_float32_buf_t dynamics_abs_vel_x; + big_float32_buf_t dynamics_abs_vel_x_std; + big_float32_buf_t dynamics_abs_vel_y; + big_float32_buf_t dynamics_abs_vel_y_std; + big_float32_buf_t dynamics_abs_vel_covariance_xy; + uint8_t dynamics_rel_vel_invalid_flags; + big_float32_buf_t dynamics_rel_vel_x; + big_float32_buf_t dynamics_rel_vel_x_std; + big_float32_buf_t dynamics_rel_vel_y; + big_float32_buf_t dynamics_rel_vel_y_std; + big_float32_buf_t dynamics_rel_vel_covariance_xy; + uint8_t dynamics_abs_accel_invalid_flags; + big_float32_buf_t dynamics_abs_accel_x; + big_float32_buf_t dynamics_abs_accel_x_std; + big_float32_buf_t dynamics_abs_accel_y; + big_float32_buf_t dynamics_abs_accel_y_std; + big_float32_buf_t dynamics_abs_accel_covariance_xy; + uint8_t dynamics_rel_accel_invalid_flags; + big_float32_buf_t dynamics_rel_accel_x; + big_float32_buf_t dynamics_rel_accel_x_std; + big_float32_buf_t dynamics_rel_accel_y; + big_float32_buf_t dynamics_rel_accel_y_std; + big_float32_buf_t dynamics_rel_accel_covariance_xy; + uint8_t dynamics_orientation_invalid_flags; + big_float32_buf_t dynamics_orientation_rate_mean; + big_float32_buf_t dynamics_orientation_rate_std; + big_uint32_buf_t shape_length_status; + uint8_t shape_length_edge_invalid_flags; + big_float32_buf_t shape_length_edge_mean; + big_float32_buf_t shape_length_edge_std; + big_uint32_buf_t shape_width_status; + uint8_t shape_width_edge_invalid_flags; + big_float32_buf_t shape_width_edge_mean; + big_float32_buf_t shape_width_edge_std; +}; + +struct ObjectListPacket +{ + HeaderPacket header; + HeaderSomeIPPacket header_some_ip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; + big_uint32_buf_t event_data_qualifier; + uint8_t extended_qualifier; + uint8_t number_of_objects; + ObjectPacket objects[MAX_OBJECTS]; +}; + +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{}; +}; + +struct SensorStatusPacket +{ + HeaderPacket header; + StampSyncStatusPacket stamp; + uint8_t sw_version_major; + uint8_t sw_version_minor; + uint8_t sw_version_patch; + StatusConfigurationPacket status; + uint8_t configuration_counter; + uint8_t longitudinal_velocity_status; + uint8_t longitudinal_acceleration_status; + uint8_t lateral_acceleration_status; + uint8_t yaw_rate_status; + uint8_t steering_angle_status; + uint8_t driving_direction_status; + uint8_t characteristic_speed_status; + uint8_t radar_status; + uint8_t voltage_status; + uint8_t temperature_status; + uint8_t blockage_status; +}; + +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{}; +}; + +struct AccelerationLateralCoGPacket +{ + HeaderPacket header; + uint8_t reserved0[6]; + big_float32_buf_t acceleration_lateral; + uint8_t reserved1[22]; +}; + +struct AccelerationLongitudinalCoGPacket +{ + HeaderPacket header; + uint8_t reserved0[6]; + big_float32_buf_t acceleration_lateral; + uint8_t reserved1[22]; +}; + +struct CharacteristicSpeedPacket +{ + HeaderPacket header; + uint8_t reserved0[2]; + uint8_t characteristic_speed; + uint8_t reserved1[8]; +}; + +struct DrivingDirectionPacket +{ + HeaderPacket header; + uint8_t reserved0; + uint8_t driving_direction; + uint8_t reserved1[20]; +}; + +struct SteeringAngleFrontAxlePacket +{ + HeaderPacket header; + uint8_t reserved0[6]; + big_float32_buf_t steering_angle_front_axle; + uint8_t reserved1[22]; +}; + +struct VelocityVehiclePacket +{ + HeaderPacket header; + uint8_t reserved0[3]; + big_float32_buf_t velocity_vehicle; + uint8_t reserved1[21]; +}; + +struct YawRatePacket +{ + HeaderPacket header; + uint8_t reserved0[6]; + big_float32_buf_t yaw_rate; + uint8_t reserved1[22]; +}; + +struct FilterStatusEntryPacket +{ + uint8_t active; + uint8_t data_index; + big_float32_buf_t min_value; + big_float32_buf_t max_value; +}; + +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]; +}; + +#pragma pack(pop) + +struct PointARS548Detection +{ + PCL_ADD_POINT4D; + float azimuth; + float azimuth_std; + float elevation; + 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; + 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/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index 154034111..9c6e5d555 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -14,7 +14,7 @@ namespace nebula namespace drivers { /// @brief struct for Hesai sensor configuration -struct HesaiSensorConfiguration : SensorConfigurationBase +struct HesaiSensorConfiguration : LidarConfigurationBase { uint16_t gnss_port{}; double scan_phase{}; @@ -33,7 +33,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 diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 563f166ab..9d04a267a 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -327,6 +327,7 @@ enum class SensorModel { ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, + CONTINENTAL_ARS548 }; /// @brief not used? @@ -436,6 +437,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; @@ -447,11 +451,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; @@ -466,12 +480,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; @@ -510,6 +543,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; } @@ -557,6 +591,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..1984642dc 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -20,7 +20,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 +36,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; diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index fd963cd65..b429e7b53 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -12,7 +12,7 @@ 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 +26,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 d08b16d6a..31385de6f 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -52,6 +52,11 @@ 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_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_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp new file mode 100644 index 000000000..139ea6d5e --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -0,0 +1,104 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +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; + + /// @brief Function for parsing detection lists + /// @param data + /// @return Resulting flag + bool ParseDetectionsListPacket( + const std::vector & data, const std_msgs::msg::Header & header); + + /// @brief Function for parsing object lists + /// @param data + /// @return Resulting flag + bool ParseObjectsListPacket( + const std::vector & data, const std_msgs::msg::Header & header); + + /// @brief Function for parsing sensor status messages + /// @param data + /// @return Resulting flag + bool ParseSensorStatusPacket( + const std::vector & data, const std_msgs::msg::Header & header); + + /// @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 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 +} // 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..e5f0f5efa --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -0,0 +1,49 @@ +// 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_WS_CONTINENTAL_PACKETS_DECODER_HPP +#define NEBULA_WS_CONTINENTAL_PACKETS_DECODER_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 +{ +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..462f3956e 100644 --- a/nebula_decoders/package.xml +++ b/nebula_decoders/package.xml @@ -13,14 +13,18 @@ ros_environment angles + continental_msgs + diagnostic_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_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp new file mode 100644 index 000000000..fb2a44f60 --- /dev/null +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -0,0 +1,623 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp" + +#include "nebula_common/continental/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; +} + +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) +{ + if (nebula_packets.packets.size() != 1) { + return false; + } + + const auto & data = nebula_packets.packets[0].data; + + if (data.size() < sizeof(HeaderPacket)) { + return false; + } + + HeaderPacket header{}; + std::memcpy(&header, data.data(), sizeof(HeaderPacket)); + + if (header.service_id.value() != 0) { + return false; + } + + 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, 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, 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; +} + +bool ContinentalARS548Decoder::ParseDetectionsListPacket( + const std::vector & data, const std_msgs::msg::Header & header) +{ + auto msg_ptr = std::make_unique(); + 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; + + 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); + + 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); + + // Estimate dropped detections only when the radar is synchronized + 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 + + 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); + 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(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); + + for (std::size_t detection_index = 0; detection_index < number_of_detections; detection_index++) { + auto & detection_msg = msg.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(); + } + + detection_list_callback_(std::move(msg_ptr)); + + return true; +} + +bool ContinentalARS548Decoder::ParseObjectsListPacket( + const std::vector & data, const std_msgs::msg::Header & header) +{ + 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_->object_frame; + + 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); + + const uint8_t number_of_objects = object_list.number_of_objects; + + msg.objects.resize(number_of_objects); + + // Estimate dropped objects only when the radar is synchronized + 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 + + 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]; + + 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.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()); + 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.existence_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(); + } + + object_list_callback_(std::move(msg_ptr)); + + return true; +} + +bool ContinentalARS548Decoder::ParseSensorStatusPacket( + 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)); + + radar_status_.timestamp_nanoseconds = sensor_status_packet.stamp.timestamp_nanoseconds.value(); + radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.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; + + 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(); + + 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(); + + 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; + + 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; + } + + 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) << "." + << 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; + + 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 = + vdy_value_to_string(sensor_status_packet.longitudinal_acceleration_status); + radar_status_.lateral_acceleration_status = + 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 = + vdy_value_to_string(sensor_status_packet.driving_direction_status); + radar_status_.characteristic_speed_status = + 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) { + voltage_status_vector.push_back("Ok"); + } + if (sensor_status_packet.voltage_status & 0x01) { + voltage_status_vector.push_back("Current undervoltage"); + } + if (sensor_status_packet.voltage_status & 0x02) { + voltage_status_vector.push_back("Past undervoltage"); + } + if (sensor_status_packet.voltage_status & 0x04) { + 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) { + temperature_status_vector.push_back("Ok"); + } + if (sensor_status_packet.temperature_status & 0x01) { + temperature_status_vector.push_back("Current undertemperature"); + } + if (sensor_status_packet.temperature_status & 0x02) { + temperature_status_vector.push_back("Past undertemperature"); + } + if (sensor_status_packet.temperature_status & 0x04) { + temperature_status_vector.push_back("Current overtemperature"); + } + if (sensor_status_packet.temperature_status & 0x08) { + temperature_status_vector.push_back("Past overtemperature"); + } + + 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; + + 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; + } + + 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++; + 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 b0e15b4d5..32893d1f4 100644 --- a/nebula_hw_interfaces/CMakeLists.txt +++ b/nebula_hw_interfaces/CMakeLists.txt @@ -38,6 +38,12 @@ 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_ars548_hw_interface.cpp + src/nebula_continental_hw_interfaces/multi_continental_ars548_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..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 @@ -22,7 +22,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; @@ -36,11 +36,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. @@ -59,7 +59,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_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp new file mode 100644 index 000000000..3e150b18e --- /dev/null +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -0,0 +1,218 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H +#define NEBULA_CONTINENTAL_ARS548_HW_INTERFACE_H +// Have to define macros to silence warnings about deprecated headers being used by +// boost/property_tree/ in some versions of boost. +// See: https://github.com/boostorg/property_tree/issues/51 +#include +#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076) // Boost 1.73 - 1.76 +#define BOOST_BIND_GLOBAL_PLACEHOLDERS +#endif +#if (BOOST_VERSION / 100 == 1074) // Boost 1.74 +#define BOOST_ALLOW_DEPRECATED_HEADERS +#endif +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace continental_ars548 +{ +/// @brief Hardware interface of the Continental ARS548 radar +class ContinentalARS548HwInterface : NebulaHwInterfaceBase +{ +private: + std::unique_ptr<::drivers::common::IoContext> sensor_io_context_; + std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_; + std::shared_ptr sensor_configuration_; + std::unique_ptr nebula_packets_ptr_; + std::function buffer)> + nebula_packets_reception_callback_; + + std::mutex sensor_status_mutex_; + + SensorStatusPacket sensor_status_packet_{}; + FilterStatusPacket filter_status_{}; + + std::shared_ptr parent_node_logger; + + /// @brief Printing the string to RCLCPP_INFO_STREAM + /// @param info Target string + void PrintInfo(std::string info); + + /// @brief Printing the string to RCLCPP_ERROR_STREAM + /// @param error Target string + void PrintError(std::string error); + + /// @brief Printing the string to RCLCPP_DEBUG_STREAM + /// @param debug Target string + void PrintDebug(std::string debug); + + /// @brief Printing the bytes to RCLCPP_DEBUG_STREAM + /// @param bytes Target byte vector + void PrintDebug(const std::vector & bytes); + +public: + /// @brief Constructor + ContinentalARS548HwInterface(); + + /// @brief Process a new filter status packet + /// @param buffer The buffer containing the status packet + void ProcessFilterStatusPacket(const std::vector & buffer); + + /// @brief Process a new data packet + /// @param buffer The buffer containing the data packet + void ProcessDataPacket(const std::vector & buffer); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallbackWithSender( + const std::vector & buffer, const std::string & sender_ip); + + /// @brief Callback function to receive the Cloud Packet data from the UDP Driver + /// @param buffer Buffer containing the data received from the UDP socket + void ReceiveSensorPacketCallback(const std::vector & buffer) final; + + /// @brief Starting the interface that handles UDP streams + /// @return Resulting status + Status SensorInterfaceStart() final; + + /// @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)> scan_callback); + + /// @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 + /// @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 Set the vehicle parameters + /// @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 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)) + /// @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 power_save_standstill Desired power_save_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 power_save_standstill); + + /// @brief Set the sensor ip address + /// @param sensor_ip_address Desired sensor ip address + /// @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 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_CONTINENTAL_ARS548_HW_INTERFACE_H 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 949c2a508..2f5639373 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 @@ -168,13 +168,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 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..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 @@ -70,7 +70,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 +78,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 +86,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..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 @@ -106,13 +106,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/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_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp new file mode 100644 index 000000000..2deeb7a28 --- /dev/null +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -0,0 +1,625 @@ +// 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 +{ +ContinentalARS548HwInterface::ContinentalARS548HwInterface() +: sensor_io_context_{new ::drivers::common::IoContext(1)}, + sensor_udp_driver_{new ::drivers::udp_driver::UdpDriver(*sensor_io_context_)}, + nebula_packets_ptr_{std::make_unique()} +{ +} + +Status ContinentalARS548HwInterface::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 ContinentalARS548HwInterface::SensorInterfaceStart() +{ + try { + sensor_udp_driver_->init_receiver( + sensor_configuration_->multicast_ip, sensor_configuration_->data_port, + sensor_configuration_->host_ip, sensor_configuration_->data_port, 2 << 16); + sensor_udp_driver_->receiver()->setMulticast(true); + sensor_udp_driver_->receiver()->open(); + sensor_udp_driver_->receiver()->bind(); + sensor_udp_driver_->receiver()->asyncReceiveWithSender(std::bind( + &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, + std::placeholders::_1, std::placeholders::_2)); + + sensor_udp_driver_->init_sender( + sensor_configuration_->sensor_ip, sensor_configuration_->configuration_sensor_port, + sensor_configuration_->host_ip, sensor_configuration_->configuration_host_port); + + sensor_udp_driver_->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 ContinentalARS548HwInterface::RegisterScanCallback( + std::function)> callback) +{ + nebula_packets_reception_callback_ = std::move(callback); + return Status::OK; +} + +void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( + const std::vector & buffer, const std::string & sender_ip) +{ + if (sender_ip == sensor_configuration_->sensor_ip) { + ReceiveSensorPacketCallback(buffer); + } +} +void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(const std::vector & buffer) +{ + if (buffer.size() < sizeof(HeaderPacket)) { + PrintError("Unrecognized packet. Too short"); + return; + } + + HeaderPacket header_packet{}; + std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); + + if (header_packet.service_id.value() != 0) { + PrintError("Invalid service id"); + return; + } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { + if ( + buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || + header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { + PrintError("SensorStatus message with invalid size"); + return; + } + ProcessDataPacket(buffer); + } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { + if ( + buffer.size() != FILTER_STATUS_UDP_PAYLOAD || + header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { + PrintError("FilterStatus message with invalid size"); + return; + } + + ProcessFilterStatusPacket(buffer); + } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + buffer.size() != DETECTION_LIST_UDP_PAYLOAD || + header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { + PrintError("DetectionList message with invalid size"); + return; + } + + ProcessDataPacket(buffer); + } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { + if ( + buffer.size() != OBJECT_LIST_UDP_PAYLOAD || + header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { + PrintError("ObjectList message with invalid size"); + return; + } + + ProcessDataPacket(buffer); + } +} + +void ContinentalARS548HwInterface::ProcessFilterStatusPacket(const std::vector & buffer) +{ + assert(buffer.size() == sizeof(FilterStatusPacket)); + std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); +} + +void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) +{ + nebula_msgs::msg::NebulaPacket nebula_packet; + nebula_packet.data = buffer; + auto now = std::chrono::system_clock::now(); + auto now_secs = std::chrono::duration_cast(now.time_since_epoch()).count(); + auto now_nanosecs = + 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 ContinentalARS548HwInterface::SensorInterfaceStop() +{ + return Status::ERROR_1; +} + +Status ContinentalARS548HwInterface::GetSensorConfiguration( + SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + PrintDebug(ss.str()); + return Status::ERROR_1; +} + +Status ContinentalARS548HwInterface::SetSensorMounting( + float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, + float pitch_autosar, uint8_t plug_orientation) +{ + 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; + } + + 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)); + + 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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalARS548HwInterface::SetVehicleParameters( + float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) +{ + 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; + } + + 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_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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +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) +{ + 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 || + power_save_standstill > 1) { + PrintError("Invalid SetRadarParameters values"); + return Status::SENSOR_CONFIG_ERROR; + } + + 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.time_slot = time_slot; + configuration_packet.configuration.hcc = hcc; + configuration_packet.configuration.powersave_standstill = power_save_standstill; + configuration_packet.new_radar_parameters = 1; + + 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("time_slot = " + std::to_string(time_slot)); + 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; +} + +Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) +{ + 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=" + sensor_ip_address); + 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; + 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_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; +} + +Status ContinentalARS548HwInterface::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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalARS548HwInterface::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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalARS548HwInterface::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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +Status ContinentalARS548HwInterface::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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +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_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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +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; + 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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +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; + 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)); + + if (!sensor_udp_driver_->sender()->isOpen()) { + return Status::ERROR_1; + } + + sensor_udp_driver_->sender()->asyncSend(send_vector); + + return Status::OK; +} + +void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) +{ + parent_node_logger = logger; +} + +void ContinentalARS548HwInterface::PrintInfo(std::string info) +{ + if (parent_node_logger) { + RCLCPP_INFO_STREAM((*parent_node_logger), info); + } else { + std::cout << info << std::endl; + } +} + +void ContinentalARS548HwInterface::PrintError(std::string error) +{ + if (parent_node_logger) { + RCLCPP_ERROR_STREAM((*parent_node_logger), error); + } else { + std::cerr << error << std::endl; + } +} + +void ContinentalARS548HwInterface::PrintDebug(std::string debug) +{ + if (parent_node_logger) { + RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + } else { + std::cout << debug << std::endl; + } +} + +void ContinentalARS548HwInterface::PrintDebug(const std::vector & bytes) +{ + std::stringstream ss; + for (const auto & b : bytes) { + ss << static_cast(b) << ", "; + } + ss << std::endl; + PrintDebug(ss.str()); +} + +} // namespace continental_ars548 +} // namespace drivers +} // namespace nebula diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/multi_continental_ars548_hw_interface.cpp 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 b35162439..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 @@ -185,7 +185,7 @@ Status HesaiHwInterface::SetSensorConfiguration( return Status::OK; } -Status HesaiHwInterface::CloudInterfaceStart() +Status HesaiHwInterface::SensorInterfaceStart() { try { std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl; @@ -204,7 +204,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 @@ -224,7 +224,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())) { @@ -274,7 +274,7 @@ void HesaiHwInterface::ReceiveCloudPacketCallback(const std::vector & b } } } -Status HesaiHwInterface::CloudInterfaceStop() +Status HesaiHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } 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..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 @@ -12,7 +12,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 +99,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 +109,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 << "," @@ -145,7 +145,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() return Status::OK; } -Status RobosenseHwInterface::CloudInterfaceStop() +Status RobosenseHwInterface::SensorInterfaceStop() { return Status::ERROR_1; } @@ -238,4 +238,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..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 @@ -36,7 +36,7 @@ Status VelodyneHwInterface::SetSensorConfiguration( return rt; } -Status VelodyneHwInterface::CloudInterfaceStart() +Status VelodyneHwInterface::SensorInterfaceStart() { try { cloud_udp_driver_->init_receiver( @@ -44,7 +44,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 +61,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 +99,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_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..f6d7e40de --- /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_probability +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..a10c1ad36 --- /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 orientation +float32 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/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/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..32a8d60f2 --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv @@ -0,0 +1,10 @@ +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 +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..72ce8025c --- /dev/null +++ b/nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv @@ -0,0 +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_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..dd81e1ac0 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -123,6 +123,33 @@ rclcpp_components_register_node(robosense_hw_monitor_ros_wrapper EXECUTABLE robosense_hw_monitor_ros_wrapper_node ) +## Continental +# Hw Interface +ament_auto_add_library(continental_ars548_hw_ros_wrapper SHARED + src/continental/continental_ars548_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(continental_ars548_hw_ros_wrapper + PLUGIN "ContinentalARS548HwInterfaceRosWrapper" + EXECUTABLE continental_ars548_hw_interface_ros_wrapper_node + ) + +ament_auto_add_library(multi_continental_ars548_hw_ros_wrapper SHARED + src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp + ) +rclcpp_components_register_node(multi_continental_ars548_hw_ros_wrapper + PLUGIN "MultiContinentalARS548HwInterfaceRosWrapper" + EXECUTABLE multi_continental_ars548_hw_interface_ros_wrapper_node + ) + +# DriverDecoder +ament_auto_add_library(continental_ars548_driver_ros_wrapper SHARED + src/continental/continental_ars548_decoder_ros_wrapper.cpp + ) +rclcpp_components_register_node(continental_ars548_driver_ros_wrapper + PLUGIN "ContinentalARS548DriverRosWrapper" + EXECUTABLE continental_ars548_driver_ros_wrapper_node + ) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() 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/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..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 @@ -34,8 +34,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/common/nebula_hw_interface_ros_wrapper_base.hpp b/nebula_ros/include/nebula_ros/common/nebula_hw_interface_ros_wrapper_base.hpp index 8f387226c..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 @@ -23,7 +23,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; @@ -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; 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 new file mode 100644 index 000000000..059388639 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_ros_wrapper.hpp @@ -0,0 +1,166 @@ +// 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_ContinentalARS548DriverRosWrapper_H +#define NEBULA_ContinentalARS548DriverRosWrapper_H + +#include +#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 Ros wrapper of continental radar ethernet driver +class ContinentalARS548DriverRosWrapper 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_; + rclcpp::Publisher::SharedPtr scan_raw_pub_; + rclcpp::Publisher::SharedPtr objects_raw_pub_; + rclcpp::Publisher::SharedPtr objects_markers_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + std::unordered_set previous_ids_; + + constexpr static int REFERENCE_POINTS_NUM = 9; + constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { + {{{-1.0, -1.0}}, + {{-1.0, 0.0}}, + {{-1.0, 1.0}}, + {{0.0, 1.0}}, + {{1.0, 1.0}}, + {{1.0, 0.0}}, + {{1.0, -1.0}}, + {{0.0, -1.0}}, + {{0.0, 0.0}}}}; + + std::shared_ptr + sensor_cfg_ptr_; + + drivers::continental_ars548::ContinentalARS548HwInterface 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::continental_ars548::ContinentalARS548SensorConfiguration & 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)); + } + + /// @brief Callback to process new ContinentalArs548DetectionList from the driver + /// @param msg The new ContinentalArs548DetectionList from the driver + void DetectionListCallback( + 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); + + /// @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); + + /// @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(); + + /// @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); + + /// @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 +} // namespace nebula + +#endif // NEBULA_ContinentalARS548DriverRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp new file mode 100644 index 000000000..f9b66e951 --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp @@ -0,0 +1,188 @@ +// 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 +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_ars548::ContinentalARS548HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + rclcpp::Publisher::SharedPtr packets_pub_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + + bool standstill_{true}; + + rclcpp::Service::SharedPtr + set_network_configuration_service_server_; + rclcpp::Service::SharedPtr + set_sensor_mounting_service_server_; + rclcpp::Service::SharedPtr + set_vehicle_parameters_service_server_; + rclcpp::Service::SharedPtr + set_radar_parameters_service_server_; + + /// @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 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); + + /// @brief Service callback to set the new sensor ip + /// @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 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 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 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); + ~ContinentalARS548HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); + +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/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..19429b99c --- /dev/null +++ b/nebula_ros/include/nebula_ros/continental/multi_continental_ars548_hw_interface_ros_wrapper.hpp @@ -0,0 +1,146 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NEBULA_ContinentalARS548HwInterfaceRosWrapper_H +#define NEBULA_ContinentalARS548HwInterfaceRosWrapper_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace nebula +{ +namespace ros +{ +/// @brief Get parameter from rclcpp::Parameter +/// @tparam T +/// @param p Parameter from rclcpp parameter callback +/// @param name Target parameter name +/// @param value Corresponding value +/// @return Whether the target name existed +template +bool get_param(const std::vector & p, const std::string & name, T & value) +{ + auto it = std::find_if(p.cbegin(), p.cend(), [&name](const rclcpp::Parameter & parameter) { + return parameter.get_name() == name; + }); + if (it != p.cend()) { + value = it->template get_value(); + return true; + } + return false; +} + +/// @brief Hardware interface ros wrapper of continental radar ethernet driver +/// NOTE: this is a temporary class that acts as a single hw interface for multiple devices +/// The reason behind this is a not-so-efficient multicasting and package processing when having N +/// interfaces for N devices If we end up having problems because of that we may switch to this +/// implementation. Otherwise this implementation will be removed at a later date +class MultiContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, + NebulaHwInterfaceWrapperBase +{ + drivers::continental_ars548::MultiContinentalARS548HwInterface hw_interface_; + Status interface_status_; + + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration sensor_configuration_; + + /// @brief Received Continental Radar message publisher + std::unordered_map::SharedPtr> + packets_pub_map_; + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::SharedPtr steering_angle_sub_; + + bool standstill_{true}; + + /// @brief Initializing hardware interface ros wrapper + /// @param sensor_configuration SensorConfiguration for this driver + /// @return Resulting status + Status InitializeHwInterface( + const drivers::SensorConfigurationBase & sensor_configuration) override; + /// @brief Callback for receiving NebulaPackets + /// @param packets_buffer Received NebulaPackets + void ReceivePacketsDataCallback( + std::unique_ptr packets_buffer, const std::string & sensor_ip); + + /// @brief Callback to send the odometry information to the radar device + /// @param msg The odometry message + void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the acceleration information to the radar device + /// @param msg The acceleration message + void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + + /// @brief Callback to send the steering angle information to the radar device + /// @param msg The steering angle message + void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + +public: + explicit MultiContinentalARS548HwInterfaceRosWrapper(const rclcpp::NodeOptions & options); + ~MultiContinentalARS548HwInterfaceRosWrapper() noexcept override; + + /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) + /// @return Resulting status + Status StreamStart() override; + /// @brief Stop point cloud streaming (not used) + /// @return Resulting status + Status StreamStop() override; + /// @brief Shutdown (not used) + /// @return Resulting status + Status Shutdown() override; + /// @brief Get configurations from ros parameters + /// @param sensor_configuration Output of SensorConfiguration + /// @return Resulting status + Status GetParameters( + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration); + +private: + std::mutex mtx_config_; + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + /// @brief rclcpp parameter callback + /// @param parameters Received parameters + /// @return SetParametersResult + rcl_interfaces::msg::SetParametersResult paramCallback( + const std::vector & parameters); + + /// @brief Updating rclcpp parameter + /// @return SetParametersResult + std::vector updateParameters(); +}; + +} // namespace ros +} // namespace nebula + +#endif // NEBULA_ContinentalARS548HwInterfaceRosWrapper_H diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index 91caca085..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 @@ -65,7 +65,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/robosense/robosense_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_decoder_ros_wrapper.hpp index 0f0b02acc..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 @@ -37,7 +37,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 +96,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..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 @@ -41,7 +41,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..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 @@ -64,7 +64,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 new file mode 100644 index 000000000..d5ed36078 --- /dev/null +++ b/nebula_ros/launch/continental_launch_all_hw.xml @@ -0,0 +1,112 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index 73c516761..b394498fb 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") @@ -56,12 +68,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, @@ -80,6 +94,9 @@ def launch_setup(context, *args, **kwargs): ], ), ) + + + if launch_hw and is_hw_monitor_available(sensor_make): nodes.append( # HwMonitor ComposableNode( @@ -101,7 +118,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, @@ -130,7 +147,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="", diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index c9c86d896..87bc000c9 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -12,8 +12,11 @@ ament_cmake_auto ros_environment + continental_msgs + continental_srvs diagnostic_msgs diagnostic_updater + geometry_msgs libpcl-all-dev nebula_common nebula_decoders @@ -21,9 +24,13 @@ pcl_conversions rclcpp rclcpp_components - yaml-cpp - velodyne_msgs robosense_msgs + tf2_eigen + tf2_geometry_msgs + tf2_ros + velodyne_msgs + visualization_msgs + yaml-cpp ament_cmake_gtest ament_lint_auto 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..ecad7752f --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_decoder_ros_wrapper.cpp @@ -0,0 +1,802 @@ +// 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 + +namespace nebula +{ +namespace ros +{ +ContinentalARS548DriverRosWrapper::ContinentalARS548DriverRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_ars548_driver_ros_wrapper", options), hw_interface_() +{ + drivers::continental_ars548::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( + "continental_detections", rclcpp::SensorDataQoS()); + object_list_pub_ = this->create_publisher( + "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()); + + objects_markers_pub_ = + this->create_publisher("marker_array", 10); + + diagnostics_pub_ = + this->create_publisher("diagnostics", 10); +} + +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)); + decoder_ptr_->RegisterSensorStatusCallback(std::bind( + &ContinentalARS548DriverRosWrapper::SensorStatusCallback, this, std::placeholders::_1)); + + return Status::OK; +} + +Status ContinentalARS548DriverRosWrapper::GetStatus() +{ + return wrapper_status_; +} + +Status ContinentalARS548DriverRosWrapper::GetParameters( + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model"); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration); + + 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 ( + 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) { + 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 ( + objects_raw_pub_->get_subscription_count() > 0 || + objects_raw_pub_->get_intra_process_subscription_count() > 0) { + auto objects_raw_msg = ConvertToRadarTracks(*msg); + objects_raw_msg.header = msg->header; + objects_raw_pub_->publish(std::move(objects_raw_msg)); + } + + if ( + objects_markers_pub_->get_subscription_count() > 0 || + objects_markers_pub_->get_intra_process_subscription_count() > 0) { + auto marker_array_msg = ConvertToMarkers(*msg); + objects_markers_pub_->publish(std::move(marker_array_msg)); + } + + if ( + object_list_pub_->get_subscription_count() > 0 || + object_list_pub_->get_intra_process_subscription_count() > 0) { + object_list_pub_->publish(std::move(msg)); + } +} + +void 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) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.detections.size()); + + 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; + point.y = + 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.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; + 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); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +pcl::PointCloud::Ptr +ContinentalARS548DriverRosWrapper::ConvertToPointcloud( + const continental_msgs::msg::ContinentalArs548ObjectList & msg) +{ + pcl::PointCloud::Ptr output_pointcloud( + new pcl::PointCloud); + output_pointcloud->reserve(msg.objects.size()); + + nebula::drivers::continental_ars548::PointARS548Object point{}; + for (const auto & object : msg.objects) { + 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; + 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); + } + + output_pointcloud->height = 1; + output_pointcloud->width = output_pointcloud->points.size(); + return output_pointcloud; +} + +radar_msgs::msg::RadarScan ContinentalARS548DriverRosWrapper::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 ContinentalARS548DriverRosWrapper::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; + constexpr float INVALID_COVARIANCE = 1e6; + + radar_msgs::msg::RadarTrack track_msg; + 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; + // 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 + + 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 = object.classification_unknown; + track_msg.classification = UNKNOWN_ID; + + if (object.classification_car > max_score) { + max_score = object.classification_car; + track_msg.classification = CAR_ID; + } + if (object.classification_truck > max_score) { + max_score = object.classification_truck; + track_msg.classification = TRUCK_ID; + } + if (object.classification_motorcycle > max_score) { + max_score = object.classification_motorcycle; + track_msg.classification = MOTORCYCLE_ID; + } + if (object.classification_bicycle > max_score) { + max_score = object.classification_bicycle; + track_msg.classification = BICYCLE_ID; + } + if (object.classification_pedestrian > max_score) { + max_score = object.classification_pedestrian; + track_msg.classification = PEDESTRIAN_ID; + } + + 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 * object.position_std.y); + track_msg.position_covariance[4] = 0.f; + 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 * 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 * object.absolute_velocity_std.y); + track_msg.velocity_covariance[4] = 0.f; + 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 * 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 * object.absolute_acceleration_std.y); + track_msg.acceleration_covariance[4] = 0.f; + 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; + track_msg.size_covariance[2] = 0.f; + track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[4] = 0.f; + track_msg.size_covariance[5] = INVALID_COVARIANCE; + + output_msg.tracks.emplace_back(track_msg); + } + + return output_msg; +} + +visualization_msgs::msg::MarkerArray 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 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_->object_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_->object_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 new file mode 100644 index 000000000..d7baa3f6a --- /dev/null +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -0,0 +1,561 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + hw_interface_.RegisterScanCallback(std::bind( + &ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1)); + packets_pub_ = this->create_publisher( + "nebula_packets", rclcpp::SensorDataQoS()); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +ContinentalARS548HwInterfaceRosWrapper::~ContinentalARS548HwInterfaceRosWrapper() +{ +} + +Status ContinentalARS548HwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::SensorDataQoS(), + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + + set_network_configuration_service_server_ = + this->create_service( + "set_network_configuration", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_sensor_mounting_service_server_ = + this->create_service( + "set_sensor_mounting", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_vehicle_parameters_service_server_ = + this->create_service( + "set_vehicle_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + set_radar_parameters_service_server_ = + this->create_service( + "set_radar_parameters", + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback, this, + std::placeholders::_1, std::placeholders::_2)); + } + + return interface_status_; +} + +Status ContinentalARS548HwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status ContinentalARS548HwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status ContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status ContinentalARS548HwInterfaceRosWrapper::GetParameters( + drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_ip", descriptor); + sensor_configuration.sensor_ip = this->get_parameter("sensor_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_length", descriptor); + sensor_configuration.configuration_vehicle_length = + static_cast(this->get_parameter("configuration_vehicle_length").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_width", descriptor); + sensor_configuration.configuration_vehicle_width = + static_cast(this->get_parameter("configuration_vehicle_width").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_height", descriptor); + sensor_configuration.configuration_vehicle_height = + static_cast(this->get_parameter("configuration_vehicle_height").as_double()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_vehicle_wheelbase", descriptor); + sensor_configuration.configuration_vehicle_wheelbase = + static_cast(this->get_parameter("configuration_vehicle_wheelbase").as_double()); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void ContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer) +{ + packets_pub_->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult ContinentalARS548HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_ars548::ContinentalARS548SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ip", new_param.sensor_ip) | get_param(p, "frame_id", new_param.frame_id) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "configuration_vehicle_length", new_param.configuration_vehicle_length) | + get_param(p, "configuration_vehicle_width", new_param.configuration_vehicle_width) | + get_param(p, "configuration_vehicle_height", new_param.configuration_vehicle_height) | + get_param(p, "configuration_vehicle_wheelbase", new_param.configuration_vehicle_wheelbase) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void ContinentalARS548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetNetworkConfigurationRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetSensorIPAddress(request->sensor_ip.data); + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback( + const std::shared_ptr request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + + auto tf_buffer = std::make_unique(this->get_clock()); + auto tf_listener = std::make_unique(*tf_buffer); + + float longitudinal = request->longitudinal; + float lateral = request->lateral; + float vertical = request->vertical; + float yaw = request->yaw; + float pitch = request->pitch; + + if (request->autoconfigure_extrinsics) { + RCLCPP_INFO( + this->get_logger(), + "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the " + "tfs"); + + geometry_msgs::msg::TransformStamped base_to_sensor_tf; + try { + base_to_sensor_tf = tf_buffer->lookupTransform( + sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0), + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)", + sensor_configuration_.frame_id.c_str(), ex.what()); + response->success = false; + response->message = ex.what(); + return; + } + + const auto & quat = base_to_sensor_tf.transform.rotation; + geometry_msgs::msg::Vector3 rpy; + tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z); + + longitudinal = base_to_sensor_tf.transform.translation.x - + sensor_configuration_.configuration_vehicle_wheelbase; + lateral = base_to_sensor_tf.transform.translation.y; + vertical = base_to_sensor_tf.transform.translation.z; + yaw = rpy.z; + pitch = rpy.y; + } + + auto result = hw_interface_.SetSensorMounting( + longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback( + [[maybe_unused]] const std::shared_ptr< + continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> + request, + const std::shared_ptr + response) +{ + float vehicle_length = request->vehicle_length; + float vehicle_width = request->vehicle_width; + float vehicle_height = request->vehicle_height; + float vehicle_wheelbase = request->vehicle_wheelbase; + + if (vehicle_length < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_length is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_length); + vehicle_length = sensor_configuration_.configuration_vehicle_length; + } + + if (vehicle_width < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_width is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_width); + vehicle_width = sensor_configuration_.configuration_vehicle_width; + } + + if (vehicle_height < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_height is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_height); + vehicle_height = sensor_configuration_.configuration_vehicle_height; + } + + if (vehicle_wheelbase < 0.f) { + RCLCPP_INFO( + this->get_logger(), + "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)", + sensor_configuration_.configuration_vehicle_wheelbase); + vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase; + } + + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetVehicleParameters( + vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +void ContinentalARS548HwInterfaceRosWrapper::SetRadarParametersRequestCallback( + const std::shared_ptr + request, + const std::shared_ptr + response) +{ + std::scoped_lock lock(mtx_config_); + auto result = hw_interface_.SetRadarParameters( + request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, + request->country_code, request->powersave_standstill); + + response->success = result == Status::OK; + response->message = (std::stringstream() << result).str(); +} + +std::vector +ContinentalARS548HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ip", sensor_configuration_.sensor_ip), + rclcpp::Parameter("frame_id", sensor_configuration_.frame_id), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port), + rclcpp::Parameter( + "configuration_vehicle_length", sensor_configuration_.configuration_vehicle_length), + rclcpp::Parameter( + "configuration_vehicle_width", sensor_configuration_.configuration_vehicle_width), + rclcpp::Parameter( + "configuration_vehicle_height", sensor_configuration_.configuration_vehicle_height), + rclcpp::Parameter( + "configuration_vehicle_wheelbase", sensor_configuration_.configuration_vehicle_wheelbase)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(ContinentalARS548HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/continental/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..b8af00fd2 --- /dev/null +++ b/nebula_ros/src/continental/multi_continental_ars548_hw_interface_ros_wrapper.cpp @@ -0,0 +1,356 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include +#include + +namespace nebula +{ +namespace ros +{ +MultiContinentalARS548HwInterfaceRosWrapper::MultiContinentalARS548HwInterfaceRosWrapper( + const rclcpp::NodeOptions & options) +: rclcpp::Node("multi_continental_ars548_hw_interface_ros_wrapper", options), hw_interface_() +{ + if (mtx_config_.try_lock()) { + interface_status_ = GetParameters(sensor_configuration_); + mtx_config_.unlock(); + } + if (Status::OK != interface_status_) { + RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error:" << interface_status_); + return; + } + hw_interface_.SetLogger(std::make_shared(this->get_logger())); + std::shared_ptr + sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + + assert(sensor_configuration_.sensor_ips.size() == sensor_configuration_.frame_ids.size()); + hw_interface_.RegisterScanCallback(std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback, this, + std::placeholders::_1, std::placeholders::_2)); + + for (std::size_t sensor_id = 0; sensor_id < sensor_configuration_.sensor_ips.size(); + sensor_id++) { + const std::string sensor_ip = sensor_configuration_.sensor_ips[sensor_id]; + const std::string frame_id = sensor_configuration_.frame_ids[sensor_id]; + packets_pub_map_[sensor_ip] = this->create_publisher( + frame_id + "/nebula_packets", rclcpp::SensorDataQoS()); + } + + set_param_res_ = this->add_on_set_parameters_callback(std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::paramCallback, this, std::placeholders::_1)); + + StreamStart(); +} + +MultiContinentalARS548HwInterfaceRosWrapper::~MultiContinentalARS548HwInterfaceRosWrapper() +{ +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStart() +{ + if (Status::OK == interface_status_) { + interface_status_ = hw_interface_.SensorInterfaceStart(); + } + + if (Status::OK == interface_status_) { + odometry_sub_ = this->create_subscription( + "odometry_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback, this, + std::placeholders::_1)); + + acceleration_sub_ = create_subscription( + "acceleration_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, + std::placeholders::_1)); + + steering_angle_sub_ = create_subscription( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, + std::placeholders::_1)); + } + + return interface_status_; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::StreamStop() +{ + return Status::OK; +} +Status MultiContinentalARS548HwInterfaceRosWrapper::Shutdown() +{ + return Status::OK; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::InitializeHwInterface( // todo: don't think + // this is needed + const drivers::SensorConfigurationBase & sensor_configuration) +{ + std::stringstream ss; + ss << sensor_configuration; + RCLCPP_DEBUG_STREAM(this->get_logger(), ss.str()); + return Status::OK; +} + +Status MultiContinentalARS548HwInterfaceRosWrapper::GetParameters( + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration & sensor_configuration) +{ + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("sensor_model", descriptor); + sensor_configuration.sensor_model = + nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("host_ip", descriptor); + sensor_configuration.host_ip = this->get_parameter("host_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("sensor_ips", descriptor); + sensor_configuration.sensor_ips = this->get_parameter("sensor_ips").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("multicast_ip", descriptor); + sensor_configuration.multicast_ip = this->get_parameter("multicast_ip").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter>("frame_ids", descriptor); + sensor_configuration.frame_ids = this->get_parameter("frame_ids").as_string_array(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("base_frame", descriptor); + sensor_configuration.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("data_port", descriptor); + sensor_configuration.data_port = this->get_parameter("data_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_host_port", descriptor); + sensor_configuration.configuration_host_port = + this->get_parameter("configuration_host_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("configuration_sensor_port", descriptor); + sensor_configuration.configuration_sensor_port = + this->get_parameter("configuration_sensor_port").as_int(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("use_sensor_time", descriptor); + sensor_configuration.use_sensor_time = this->get_parameter("use_sensor_time").as_bool(); + } + + if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { + return Status::INVALID_SENSOR_MODEL; + } + + RCLCPP_INFO_STREAM(this->get_logger(), "SensorConfig:" << sensor_configuration); + return Status::OK; +} + +void MultiContinentalARS548HwInterfaceRosWrapper::ReceivePacketsDataCallback( + std::unique_ptr scan_buffer, const std::string & sensor_ip) +{ + packets_pub_map_[sensor_ip]->publish(std::move(scan_buffer)); +} + +rcl_interfaces::msg::SetParametersResult MultiContinentalARS548HwInterfaceRosWrapper::paramCallback( + const std::vector & p) +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback"); + RCLCPP_DEBUG_STREAM(this->get_logger(), p); + RCLCPP_DEBUG_STREAM(this->get_logger(), sensor_configuration_); + RCLCPP_INFO_STREAM(this->get_logger(), p); + + drivers::continental_ars548::MultiContinentalARS548SensorConfiguration new_param{ + sensor_configuration_}; + RCLCPP_INFO_STREAM(this->get_logger(), new_param); + std::string sensor_model_str; + + if ( + get_param(p, "sensor_model", sensor_model_str) | get_param(p, "host_ip", new_param.host_ip) | + get_param(p, "sensor_ips", new_param.sensor_ips) | + get_param(p, "frame_ids", new_param.frame_ids) | + get_param(p, "data_port", new_param.data_port) | + get_param(p, "multicast_ip", new_param.multicast_ip) | + get_param(p, "base_frame", new_param.base_frame) | + get_param(p, "object_frame", new_param.object_frame) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port) | + get_param(p, "configuration_host_port", new_param.configuration_host_port) | + get_param(p, "configuration_sensor_port", new_param.configuration_sensor_port)) { + if (0 < sensor_model_str.length()) + new_param.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_str); + + sensor_configuration_ = new_param; + RCLCPP_INFO_STREAM(this->get_logger(), "Update sensor_configuration"); + std::shared_ptr sensor_cfg_ptr = + std::make_shared( + sensor_configuration_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "hw_interface_.SetSensorConfiguration"); + hw_interface_.SetSensorConfiguration( + std::static_pointer_cast(sensor_cfg_ptr)); + hw_interface_.CheckAndSetConfig(); + } + + auto result = std::make_shared(); + result->successful = true; + result->reason = "success"; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "add_on_set_parameters_callback success"); + + return *result; +} + +void MultiContinentalARS548HwInterfaceRosWrapper::OdometryCallback( + const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + + constexpr float speed_to_standstill = 0.5f; + constexpr float speed_to_moving = 2.f; + + if (standstill_ && std::abs(msg->twist.twist.linear.x) > speed_to_moving) { + standstill_ = false; + } else if (!standstill_ && std::abs(msg->twist.twist.linear.x) < speed_to_standstill) { + standstill_ = true; + } + + if (standstill_) { + hw_interface_.SetDrivingDirection(0); + } else { + hw_interface_.SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + } + + constexpr float ms_to_kmh = 3.6f; + hw_interface_.SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetYawRate(rad_to_deg * msg->twist.twist.angular.z); +} + +void MultiContinentalARS548HwInterfaceRosWrapper::AccelerationCallback( + const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + hw_interface_.SetAccelerationLateralCog(msg->accel.accel.linear.y); + hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); +} + +void MultiContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback( + const std_msgs::msg::Float32::SharedPtr msg) +{ + std::scoped_lock lock(mtx_config_); + constexpr float rad_to_deg = 180.f / M_PI; + hw_interface_.SetSteeringAngleFrontAxle(rad_to_deg * msg->data); +} + +std::vector +MultiContinentalARS548HwInterfaceRosWrapper::updateParameters() +{ + std::scoped_lock lock(mtx_config_); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters start"); + std::ostringstream os_sensor_model; + os_sensor_model << sensor_configuration_.sensor_model; + RCLCPP_INFO_STREAM(this->get_logger(), "set_parameters"); + auto results = set_parameters( + {rclcpp::Parameter("sensor_model", os_sensor_model.str()), + rclcpp::Parameter("host_ip", sensor_configuration_.host_ip), + rclcpp::Parameter("sensor_ips", sensor_configuration_.sensor_ips), + rclcpp::Parameter("frame_ids", sensor_configuration_.frame_ids), + rclcpp::Parameter("data_port", sensor_configuration_.data_port), + rclcpp::Parameter("multicast_ip", sensor_configuration_.multicast_ip), + rclcpp::Parameter("base_frame", sensor_configuration_.base_frame), + rclcpp::Parameter("object_frame", sensor_configuration_.object_frame), + rclcpp::Parameter("configuration_host_port", sensor_configuration_.configuration_host_port), + rclcpp::Parameter( + "configuration_sensor_port", sensor_configuration_.configuration_sensor_port)}); + RCLCPP_DEBUG_STREAM(this->get_logger(), "updateParameters end"); + return results; +} + +RCLCPP_COMPONENTS_REGISTER_NODE(MultiContinentalARS548HwInterfaceRosWrapper) +} // namespace ros +} // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp index 381a214f0..1ac59b55b 100644 --- a/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_hw_interface_ros_wrapper.cpp @@ -154,7 +154,7 @@ HesaiHwInterfaceRosWrapper::~HesaiHwInterfaceRosWrapper() { Status HesaiHwInterfaceRosWrapper::StreamStart() { if (Status::OK == interface_status_) { - interface_status_ = hw_interface_.CloudInterfaceStart(); + interface_status_ = hw_interface_.SensorInterfaceStart(); } return interface_status_; } diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index d779e3bc6..9025ab080 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -58,7 +58,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_); } } 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..773249813 100644 --- a/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_hw_interface_ros_wrapper.cpp @@ -42,7 +42,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_; @@ -166,4 +166,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/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp index d70a5e045..cbbc19e05 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -57,7 +57,7 @@ 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_; } 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..d31398c5b --- /dev/null +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -0,0 +1,289 @@ +// 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.base_frame = this->get_parameter("base_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("object_frame", "some_object_frame", descriptor); + sensor_configuration.object_frame = this->get_parameter("object_frame").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("frame_id", "some_sensor_frame", descriptor); + sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter( + "bag_path", (bag_root_dir / "ars548" / "1708578204").string(), descriptor); + bag_path = this->get_parameter("bag_path").as_string(); + std::cout << bag_path << std::endl; + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("storage_id", "sqlite3", descriptor); + storage_id = this->get_parameter("storage_id").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("format", "cdr", descriptor); + format = this->get_parameter("format").as_string(); + } + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = 4; + descriptor.read_only = true; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter( + "target_topic", "/sensing/radar/front_center/nebula_packets", descriptor); + target_topic = this->get_parameter("target_topic").as_string(); + } + + 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_->object_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..68d13d2dc --- /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_object_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..2b65eacfc --- /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_sensor_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