From 475a057fb8c77710cb3f93447df8dc4f5bcdf03b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 20 Dec 2023 17:13:13 +0900 Subject: [PATCH] chore: removed the old implementation Signed-off-by: Kenzo Lobos-Tsunekawa --- .../continental/continental_ars548.hpp | 337 ++----- .../decoders/continental_ars548_decoder.cpp | 903 +----------------- .../continental_ars548_hw_interface.hpp | 15 +- .../continental_types.hpp | 28 +- .../continental_ars548_hw_interface.cpp | 864 ++++------------- ...nental_ars548_hw_interface_ros_wrapper.cpp | 14 +- 6 files changed, 388 insertions(+), 1773 deletions(-) diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index e728a961e..a9de08fa1 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -32,18 +32,46 @@ namespace drivers namespace continental_ars548 { -using namespace boost::endian; +using boost::endian::big_float32_buf_t; +using boost::endian::big_uint16_buf_t; +using boost::endian::big_uint32_buf_t; +using boost::endian::big_uint64_buf_t; + +constexpr int CONFIGURATION_SERVICE_ID = 0; +constexpr int CONFIGURATION_METHOD_ID = 390; +constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; +constexpr int CONFIGURATION_UDP_LENGTH = 64; + +constexpr int DETECTION_LIST_METHOD_ID = 336; +constexpr int OBJECT_LIST_METHOD_ID = 329; +constexpr int SENSOR_STATUS_METHOD_ID = 380; +constexpr int FILTER_STATUS_METHOD_ID = 396; + +constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; +constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; +constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; +constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; + +constexpr int DETECTION_LIST_PDU_LENGTH = 35328; +constexpr int OBJECT_LIST_PDU_LENGTH = 9393; +constexpr int SENSOR_STATUS_PDU_LENGTH = 76; +constexpr int FILTER_STATUS_PDU_LENGTH = 322; + +constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; +constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; +constexpr int MAX_DETECTIONS = 800; +constexpr int MAX_OBJECTS = 50; #pragma pack(push, 1) -struct Header +struct HeaderPacket { big_uint16_buf_t service_id; big_uint16_buf_t method_id; big_uint32_buf_t length; }; -struct HeaderSOMEIP +struct HeaderSOMEIPPacket { big_uint16_buf_t client_id; big_uint16_buf_t session_id; @@ -53,7 +81,7 @@ struct HeaderSOMEIP uint8_t return_code; }; -struct HeaderE2EP07 +struct HeaderE2EP07Packet { big_uint64_buf_t crc; big_uint32_buf_t length; @@ -61,15 +89,15 @@ struct HeaderE2EP07 big_uint32_buf_t data_id; }; -struct StampSyncStatus +struct StampSyncStatusPacket { big_uint32_buf_t timestamp_nanoseconds; big_uint32_buf_t timestamp_seconds; uint8_t timestamp_sync_status; }; -struct Detection -{ // Actual 44 . Datasheet is 44 +struct DetectionPacket +{ big_float32_buf_t azimuth_angle; big_float32_buf_t azimuth_angle_std; uint8_t invalid_flags; @@ -89,12 +117,12 @@ struct Detection big_uint16_buf_t sort_index; }; -struct DetectionList +struct DetectionListPacket { - Header header; - HeaderSOMEIP header_someip; - HeaderE2EP07 header_e2ep07; - StampSyncStatus stamp; + HeaderPacket header; + HeaderSOMEIPPacket header_someip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; uint8_t extended_qualifier; big_uint16_buf_t origin_invalid_flags; @@ -111,7 +139,7 @@ struct DetectionList big_float32_buf_t origin_yaw; big_float32_buf_t origin_yaw_std; uint8_t list_invalid_flags; - Detection detections[800]; + DetectionPacket detections[MAX_DETECTIONS]; big_float32_buf_t list_rad_vel_domain_min; big_float32_buf_t list_rad_vel_domain_max; big_uint32_buf_t number_of_detections; @@ -121,9 +149,9 @@ struct DetectionList uint8_t reserverd[14]; }; -struct Object -{ // Datasheet 187 Current 184. Datsheet: 32x40 + 16x3 + 8x21 - big_uint16_buf_t status_sensor; // Current 32x40 16x3 21x1 +struct ObjectPacket +{ + big_uint16_buf_t status_sensor; big_uint32_buf_t id; big_uint16_buf_t age; uint8_t status_measurement; @@ -189,19 +217,19 @@ struct Object big_float32_buf_t shape_width_edge_std; }; -struct ObjectList +struct ObjectListPacket { - Header header; - HeaderSOMEIP header_someip; - HeaderE2EP07 header_e2ep07; - StampSyncStatus stamp; + HeaderPacket header; + HeaderSOMEIPPacket header_someip; + HeaderE2EP07Packet header_e2ep07; + StampSyncStatusPacket stamp; big_uint32_buf_t event_data_qualifier; uint8_t extended_qualifier; uint8_t number_of_objects; - Object objects[50]; + ObjectPacket objects[MAX_OBJECTS]; }; -struct StatusConfiguration +struct StatusConfigurationPacket { big_float32_buf_t longitudinal; big_float32_buf_t lateral; @@ -229,269 +257,114 @@ struct StatusConfiguration uint8_t sensor_ip_address13; }; -struct SensorStatus -{ // Actual 44 + 2 + 30 = 76 bytes. datasheet: 76 - Header header; - StampSyncStatus stamp; +struct SensorStatusPacket +{ + HeaderPacket header; + StampSyncStatusPacket stamp; uint8_t sw_version_major; uint8_t sw_version_minor; uint8_t sw_version_patch; - StatusConfiguration status; + StatusConfigurationPacket status; uint8_t configuration_counter; - uint8_t status_longitudinal_velocity; - uint8_t status_longitudinal_acceleration; - uint8_t status_lateral_acceleration; - uint8_t status_yaw_rate; - uint8_t status_steering_angle; - uint8_t status_driving_direction; - uint8_t status_characteristic_speed; - uint8_t status_radar_status; - uint8_t status_voltage_status; - uint8_t status_temperature_status; - uint8_t status_blockage_status; + uint8_t longitudinal_velocity_status; + uint8_t longitudinal_acceleration_status; + uint8_t lateral_acceleration_status; + uint8_t yaw_rate_status; + uint8_t steering_angle_status; + uint8_t driving_direction_status; + uint8_t characteristic_speed_status; + uint8_t radar_status; + uint8_t voltage_status; + uint8_t temperature_status; + uint8_t blockage_status; }; -struct Configuration +struct ConfigurationPacket { - Header header; - StatusConfiguration configuration; + HeaderPacket header; + StatusConfigurationPacket configuration; uint8_t new_sensor_mounting; uint8_t new_vehicle_parameters; uint8_t new_radar_parameters; uint8_t new_network_configuration; }; -struct AccelerationLateralCoG +struct AccelerationLateralCoGPacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t acceleration_lateral; uint8_t reserved1[22]; }; -struct AccelerationLongitudinalCoG +struct AccelerationLongitudinalCoGPacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t acceleration_lateral; uint8_t reserved1[22]; }; -struct CharasteristicSpeed +struct CharasteristicSpeedPacket { - Header header; + HeaderPacket header; uint8_t reserved0[2]; - big_float32_buf_t characteristic_speed; + uint8_t characteristic_speed; uint8_t reserved1[8]; }; -struct DrivingDirection +struct DrivingDirectionPacket { - Header header; + HeaderPacket header; uint8_t reserved0; - big_float32_buf_t driving_direction; + uint8_t driving_direction; uint8_t reserved1[20]; }; -struct SteeringAngleFrontAxle +struct SteeringAngleFrontAxlePacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t steering_angle_front_axle; uint8_t reserved1[22]; }; -struct VelocityVehicle +struct VelocityVehiclePacket { - Header header; + HeaderPacket header; uint8_t reserved0[3]; big_float32_buf_t velocity_vehicle; uint8_t reserved1[21]; }; -struct YawRate +struct YawRatePacket { - Header header; + HeaderPacket header; uint8_t reserved0[6]; big_float32_buf_t yaw_rate; - uint8_t reserved1[221]; + uint8_t reserved1[22]; }; -#pragma pack(pop) - -constexpr int SERVICE_ID_BYTE = 0; -constexpr int METHOD_ID_BYTE = 2; -constexpr int LENGTH_BYTE = 4; - -constexpr int CONFIGURATION_SERVICE_ID = 0; -constexpr int CONFIGURATION_METHOD_ID = 390; -constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; - -constexpr int STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; -constexpr int STATUS_TIMESTAMP_SECONDS_BYTE = 12; -constexpr int STATUS_SYNC_STATUS_BYTE = 16; -constexpr int STATUS_SW_VERSION_MAJOR_BYTE = 17; -constexpr int STATUS_SW_VERSION_MINOR_BYTE = 18; -constexpr int STATUS_SW_VERSION_PATCH_BYTE = 19; - -constexpr int STATUS_LONGITUDINAL_BYTE = 20; -constexpr int STATUS_LATERAL_BYTE = 24; -constexpr int STATUS_VERTICAL_BYTE = 28; -constexpr int STATUS_YAW_BYTE = 32; -constexpr int STATUS_PITCH_BYTE = 36; - -constexpr int STATUS_PLUG_ORIENTATION_BYTE = 40; -constexpr int STATUS_LENGTH_BYTE = 41; -constexpr int STATUS_WIDTH_BYTE = 45; -constexpr int STATUS_HEIGHT_BYTE = 49; -constexpr int STATUS_WHEEL_BASE_BYTE = 53; -constexpr int STATUS_MAXIMUM_DISTANCE_BYTE = 57; -constexpr int STATUS_FREQUENCY_SLOT_BYTE = 59; -constexpr int STATUS_CYCLE_TIME_BYTE = 60; -constexpr int STATUS_TIME_SLOT_BYTE = 61; -constexpr int STATUS_HCC_BYTE = 62; -constexpr int STATUS_POWER_SAVING_STANDSTILL_BYTE = 63; -constexpr int STATUS_SENSOR_IP_ADDRESS0_BYTE = 64; -constexpr int STATUS_SENSOR_IP_ADDRESS1_BYTE = 68; -constexpr int STATUS_CONFIGURATION_COUNTER_BYTE = 72; -constexpr int STATUS_LONGITUDINAL_VELOCITY_BYTE = 73; -constexpr int STATUS_LONGITUDINAL_ACCELERATION_BYTE = 74; -constexpr int STATUS_LATERAL_ACCELERATION_BYTE = 75; -constexpr int STATUS_YAW_RATE_BYTE = 76; -constexpr int STATUS_STEERING_ANGLE_BYTE = 77; -constexpr int STATUS_DRIVING_DIRECTION_BYTE = 78; -constexpr int STATUS_CHARACTERISTIC_SPEED_BYTE = 79; -constexpr int STATUS_RADAR_STATUS_BYTE = 80; -constexpr int STATUS_VOLTAGE_STATUS_BYTE = 81; -constexpr int STATUS_TEMPERATURE_STATUS_BYTE = 82; -constexpr int STATUS_BLOCKAGE_STATUS_BYTE = 83; - -constexpr int DETECTION_LIST_METHOD_ID = 336; -constexpr int OBJECT_LIST_METHOD_ID = 329; -constexpr int SENSOR_STATUS_METHOD_ID = 380; -constexpr int FILTER_STATUS_METHOD_ID = 396; - -constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; -constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; -constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; -constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; +struct FilterStatusEntryPacket +{ + uint8_t active; + uint8_t data_index; + big_float32_buf_t min_value; + big_float32_buf_t max_value; +}; -constexpr int DETECTION_LIST_PDU_LENGTH = 35328; -constexpr int OBJECT_LIST_PDU_LENGTH = 9393; -constexpr int SENSOR_STATUS_PDU_LENGTH = 76; -constexpr int FILTER_STATUS_PDU_LENGTH = 322; +struct FilterStatusPacket +{ + HeaderPacket header; + StampSyncStatusPacket stamp; + uint8_t filter_configuration_counter; + uint8_t detection_sort_index; + uint8_t object_sort_index; + FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; + FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; +}; -constexpr int DETECTION_LIST_CRC_BYTE = 16; -constexpr int DETECTION_LIST_LENGTH_BYTE = 24; -constexpr int DETECTION_LIST_SQC_BYTE = 28; -constexpr int DETECTION_LIST_DATA_ID_BYTE = 32; -constexpr int DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; -constexpr int DETECTION_LIST_TIMESTAMP_SECONDS_BYTE = 40; -constexpr int DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; -constexpr int DETECTION_LIST_ORIGIN_X_POS_BYTE = 52; -constexpr int DETECTION_LIST_ORIGIN_Y_POS_BYTE = 60; -constexpr int DETECTION_LIST_ORIGIN_Z_POS_BYTE = 68; -constexpr int DETECTION_LIST_PITCH_BYTE = 84; -constexpr int DETECTION_LIST_PITCH_STD_BYTE = 88; -constexpr int DETECTION_LIST_YAW_BYTE = 92; -constexpr int DETECTION_LIST_YAW_STD_BYTE = 96; -constexpr int DETECTION_LIST_ARRAY_BYTE = 101; -constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE = 35301; -constexpr int DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE = 35305; -constexpr int DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE = 35309; -constexpr int DETECTION_LIST_AZIMUTH_CORRECTION_BYTE = 35313; -constexpr int DETECTION_LIST_ELEVATION_CORRECTION_BYTE = 35317; -constexpr int DETECTION_LIST_ALIGNMENT_STATUS_BYTE = 35321; - -constexpr int DETECTION_STRUCT_SIZE = 44; -constexpr int DETECTION_AZIMUTH_ANGLE_BYTE = 0; -constexpr int DETECTION_AZIMUTH_ANGLE_STD_BYTE = 4; -constexpr int DETECTION_INVALID_FLAGS_BYTE = 8; -constexpr int DETECTION_ELEVATION_ANGLE_BYTE = 9; -constexpr int DETECTION_ELEVATION_ANGLE_STD_BYTE = 13; -constexpr int DETECTION_RANGE_BYTE = 17; -constexpr int DETECTION_RANGE_STD_BYTE = 21; -constexpr int DETECTION_RANGE_RATE_BYTE = 25; -constexpr int DETECTION_RANGE_RATE_STD_BYTE = 29; -constexpr int DETECTION_RCS_BYTE = 33; -constexpr int DETECTION_MEASUREMENT_ID_BYTE = 34; -constexpr int DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = 36; -constexpr int DETECTION_CLASSIFICATION_BYTE = 37; -constexpr int DETECTION_MULT_TARGET_PROBABILITY_BYTE = 38; -constexpr int DETECTION_OBJECT_ID_BYTE = 39; -constexpr int DETECTION_AMBIGUITY_FLAG_BYTE = 41; - -constexpr int OBJECT_LIST_CRC_BYTE = 16; -constexpr int OBJECT_LIST_LENGTH_BYTE = 24; -constexpr int OBJECT_LIST_SQC_BYTE = 28; -constexpr int OBJECT_LIST_DATA_ID_BYTE = 32; -constexpr int OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE = 36; -constexpr int OBJECT_LIST_TIMESTAMP_SECONDS_BYTE = 40; -constexpr int OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE = 44; -constexpr int OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE = 50; -constexpr int OBJECT_LIST_ARRAY_BYTE = 51; - -constexpr int OBJECT_STRUCT_SIZE = 187; -constexpr int OBJECT_STATUS_SENSOR_BYTE = 0; -constexpr int OBJECT_ID_BYTE = 2; -constexpr int OBJECT_AGE_BYTE = 6; -constexpr int OBJECT_STATUS_MEASUREMENT_BYTE = 8; -constexpr int OBJECT_STATUS_MOVEMENT_BYTE = 9; -constexpr int OBJECT_POSITION_REFERENCE_BYTE = 12; -constexpr int OBJECT_POSITION_X_BYTE = 13; -constexpr int OBJECT_POSITION_X_STD_BYTE = 17; -constexpr int OBJECT_POSITION_Y_BYTE = 21; -constexpr int OBJECT_POSITION_Y_STD_BYTE = 25; -constexpr int OBJECT_POSITION_Z_BYTE = 29; -constexpr int OBJECT_POSITION_Z_STD_BYTE = 33; - -constexpr int OBJECT_POSITION_COVARIANCE_XY_BYTE = 37; -constexpr int OBJECT_ORIENTATION_BYTE = 41; -constexpr int OBJECT_ORIENTATION_STD_BYTE = 45; - -constexpr int OBJECT_EXISTENCE_PROBABILITY_BYTE = 50; - -constexpr int OBJECT_CLASSIFICATION_CAR_BYTE = 58; -constexpr int OBJECT_CLASSIFICATION_TRUCK_BYTE = 59; -constexpr int OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = 60; -constexpr int OBJECT_CLASSIFICATION_BICYCLE_BYTE = 61; -constexpr int OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = 62; -constexpr int OBJECT_CLASSIFICATION_ANIMAL_BYTE = 63; -constexpr int OBJECT_CLASSIFICATION_HAZARD_BYTE = 64; -constexpr int OBJECT_CLASSIFICATION_UNKNOWN_BYTE = 65; - -constexpr int OBJECT_DYNAMICS_ABS_VEL_X_BYTE = 69; -constexpr int OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = 73; -constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = 77; -constexpr int OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = 81; -constexpr int OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = 85; - -constexpr int OBJECT_DYNAMICS_REL_VEL_X_BYTE = 90; -constexpr int OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = 94; -constexpr int OBJECT_DYNAMICS_REL_VEL_Y_BYTE = 98; -constexpr int OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = 102; -constexpr int OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = 106; - -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = 111; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = 115; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = 119; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = 123; -constexpr int OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = 127; - -constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = 132; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = 136; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = 140; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = 144; -constexpr int OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = 148; - -constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = 153; -constexpr int OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = 157; - -constexpr int OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = 166; -constexpr int OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = 179; - -static constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; -static constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; +#pragma pack(pop) } // namespace continental_ars548 } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index c0118fe82..c9e46a30d 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -57,31 +57,27 @@ bool ContinentalARS548Decoder::ProcessPackets( const auto & data = nebula_packets.packets[0].data; - if (data.size() < LENGTH_BYTE + sizeof(uint32_t)) { + if (data.size() < sizeof(HeaderPacket)) { return false; } - const uint16_t service_id = - (static_cast(data[SERVICE_ID_BYTE]) << 8) | data[SERVICE_ID_BYTE + 1]; - const uint16_t method_id = - (static_cast(data[METHOD_ID_BYTE]) << 8) | data[METHOD_ID_BYTE + 1]; - const uint32_t length = (static_cast(data[LENGTH_BYTE]) << 24) | - (static_cast(data[LENGTH_BYTE + 1]) << 16) | - (static_cast(data[LENGTH_BYTE + 2]) << 8) | - static_cast(data[LENGTH_BYTE + 3]); + HeaderPacket header{}; + std::memcpy(&header, data.data(), sizeof(HeaderPacket)); - if (service_id != 0) { + if (header.service_id.value() != 0) { return false; } - if (method_id == DETECTION_LIST_METHOD_ID) { - if (data.size() != DETECTION_LIST_UDP_PAYLOAD || length != DETECTION_LIST_PDU_LENGTH) { + if (header.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + data.size() != DETECTION_LIST_UDP_PAYLOAD || + header.length.value() != DETECTION_LIST_PDU_LENGTH) { return false; } return ParseDetectionsListPacket(data); - } else if (method_id == OBJECT_LIST_METHOD_ID) { - if (data.size() != OBJECT_LIST_UDP_PAYLOAD || length != OBJECT_LIST_PDU_LENGTH) { + } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { + if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { return false; } @@ -96,118 +92,40 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector(); auto & msg = *msg_ptr; + DetectionListPacket detection_list; + assert(sizeof(DetectionListPacket) == data.size()); + + std::memcpy(&detection_list, data.data(), sizeof(DetectionListPacket)); + msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.stamp.nanosec = - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - data[DETECTION_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; - msg.header.stamp.sec = - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - data[DETECTION_LIST_TIMESTAMP_SECONDS_BYTE + 3]; - msg.stamp_sync_status = data[DETECTION_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + msg.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); + msg.stamp_sync_status = detection_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); - const uint32_t origin_pos_x_u = - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_X_POS_BYTE + 3]; - const uint32_t origin_pos_y_u = - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_Y_POS_BYTE + 3]; - const uint32_t origin_pos_z_u = - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 2]) << 8) | - data[DETECTION_LIST_ORIGIN_Z_POS_BYTE + 3]; - - float origin_pos_x_f, origin_pos_y_f, origin_pos_z_f; - - std::memcpy(&origin_pos_x_f, &origin_pos_x_u, sizeof(origin_pos_x_u)); - std::memcpy(&origin_pos_y_f, &origin_pos_y_u, sizeof(origin_pos_y_u)); - std::memcpy(&origin_pos_z_f, &origin_pos_z_u, sizeof(origin_pos_z_u)); - - msg.origin_pos.x = static_cast(origin_pos_x_f); - msg.origin_pos.y = static_cast(origin_pos_y_f); - msg.origin_pos.z = static_cast(origin_pos_z_f); - - const uint32_t origin_pitch_u = - (static_cast(data[DETECTION_LIST_PITCH_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_PITCH_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_PITCH_BYTE + 2]) << 8) | - data[DETECTION_LIST_PITCH_BYTE + 3]; - const uint32_t origin_pitch_std_u = - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_PITCH_STD_BYTE + 2]) << 8) | - data[DETECTION_LIST_PITCH_STD_BYTE + 3]; - const uint32_t origin_yaw_u = (static_cast(data[DETECTION_LIST_YAW_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_YAW_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_YAW_BYTE + 2]) << 8) | - data[DETECTION_LIST_YAW_BYTE + 3]; - const uint32_t origin_yaw_std_u = - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_YAW_STD_BYTE + 2]) << 8) | - data[DETECTION_LIST_YAW_STD_BYTE + 3]; - - std::memcpy(&msg.origin_pitch, &origin_pitch_u, sizeof(origin_pitch_u)); - std::memcpy(&msg.origin_pitch_std, &origin_pitch_std_u, sizeof(origin_pitch_std_u)); - std::memcpy(&msg.origin_yaw, &origin_yaw_u, sizeof(origin_yaw_u)); - std::memcpy(&msg.origin_yaw_std, &origin_yaw_std_u, sizeof(origin_yaw_std_u)); - - const uint32_t ambiguity_free_velocity_min_u = - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 2]) << 8) | - data[DETECTION_LIST_RAD_VEL_DOMAIN_MIN_BYTE + 3]; - const uint32_t ambiguity_free_velocity_max_u = - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 2]) << 8) | - data[DETECTION_LIST_RAD_VEL_DOMAIN_MAX_BYTE + 3]; - - std::memcpy( - &msg.ambiguity_free_velocity_min, &ambiguity_free_velocity_min_u, - sizeof(ambiguity_free_velocity_min_u)); - std::memcpy( - &msg.ambiguity_free_velocity_max, &ambiguity_free_velocity_max_u, - sizeof(ambiguity_free_velocity_max_u)); - - const uint32_t alignment_azimuth_correction_u = - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 2]) << 8) | - data[DETECTION_LIST_AZIMUTH_CORRECTION_BYTE + 3]; - const uint32_t alignment_elevation_correction_u = - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 2]) << 8) | - data[DETECTION_LIST_ELEVATION_CORRECTION_BYTE + 3]; - - std::memcpy( - &msg.alignment_azimuth_correction, &alignment_azimuth_correction_u, - sizeof(alignment_azimuth_correction_u)); - std::memcpy( - &msg.alignment_elevation_correction, &alignment_elevation_correction_u, - sizeof(alignment_elevation_correction_u)); - - msg.alignment_status = data[DETECTION_LIST_ALIGNMENT_STATUS_BYTE]; - - const uint32_t number_of_detections = - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE]) << 24) | - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 1]) << 16) | - (static_cast(data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 2]) << 8) | - data[DETECTION_LIST_NUMBER_OF_DETECTIONS_BYTE + 3]; - - assert(origin_pos_x_f >= -10.f && origin_pos_x_f <= 10.f); - assert(origin_pos_y_f >= -10.f && origin_pos_y_f <= 10.f); - assert(origin_pos_z_f >= -10.f && origin_pos_z_f <= 10.f); + msg.origin_pos.x = detection_list.origin_x_pos.value(); + msg.origin_pos.y = detection_list.origin_y_pos.value(); + msg.origin_pos.z = detection_list.origin_z_pos.value(); + + msg.origin_pitch = detection_list.origin_pitch.value(); + msg.origin_pitch_std = detection_list.origin_pitch_std.value(); + msg.origin_yaw = detection_list.origin_yaw.value(); + msg.origin_yaw_std = detection_list.origin_yaw_std.value(); + + msg.ambiguity_free_velocity_min = detection_list.list_rad_vel_domain_min.value(); + msg.ambiguity_free_velocity_max = detection_list.list_rad_vel_domain_max.value(); + + msg.alignment_azimuth_correction = detection_list.alignment_azimuth_correction.value(); + msg.alignment_elevation_correction = detection_list.alignment_elevation_correction.value(); + + msg.alignment_status = detection_list.alignment_status; + + const uint32_t number_of_detections = detection_list.number_of_detections.value(); + msg.detections.resize(number_of_detections); + + assert(msg.origin_pos.x >= -10.f && msg.origin_pos.x <= 10.f); + assert(msg.origin_pos.y >= -10.f && msg.origin_pos.y <= 10.f); + assert(msg.origin_pos.z >= -10.f && msg.origin_pos.z <= 10.f); assert(msg.origin_pitch >= -M_PI && msg.origin_pitch <= M_PI); assert(msg.origin_yaw >= -M_PI && msg.origin_yaw <= M_PI); assert(msg.ambiguity_free_velocity_min >= -100.f && msg.ambiguity_free_velocity_min <= 100.f); @@ -216,187 +134,8 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector= -M_PI && msg.alignment_azimuth_correction <= M_PI); assert(msg.alignment_elevation_correction >= -M_PI && msg.alignment_elevation_correction <= M_PI); - msg.detections.resize(number_of_detections); - for (std::size_t detection_index = 0; detection_index < number_of_detections; detection_index++) { auto & detection_msg = msg.detections[detection_index]; - - const int CURRENT_DETECTION_START_BYTE = - DETECTION_LIST_ARRAY_BYTE + detection_index * DETECTION_STRUCT_SIZE; - const int CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_BYTE; - const int CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AZIMUTH_ANGLE_STD_BYTE; - const int CURRENT_DETECTION_INVALID_FLAGS_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_INVALID_FLAGS_BYTE; - const int CURRENT_DETECTION_ELEVATION_ANGLE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_BYTE; - const int CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_ELEVATION_ANGLE_STD_BYTE; - const int CURRENT_DETECTION_RANGE_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_BYTE; - const int CURRENT_DETECTION_RANGE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_STD_BYTE; - const int CURRENT_DETECTION_RANGE_RATE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_BYTE; - const int CURRENT_DETECTION_RANGE_RATE_STD_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_RANGE_RATE_STD_BYTE; - const int CURRENT_DETECTION_RCS_BYTE = CURRENT_DETECTION_START_BYTE + DETECTION_RCS_BYTE; - const int CURRENT_DETECTION_MEASUREMENT_ID_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_MEASUREMENT_ID_BYTE; - const int CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE; - const int CURRENT_DETECTION_CLASSIFICATION_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_CLASSIFICATION_BYTE; - const int CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_MULT_TARGET_PROBABILITY_BYTE; - const int CURRENT_DETECTION_OBJECT_ID_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_OBJECT_ID_BYTE; - const int CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE = - CURRENT_DETECTION_START_BYTE + DETECTION_AMBIGUITY_FLAG_BYTE; - - const uint32_t azimuth_angle_u = - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_AZIMUTH_ANGLE_BYTE + 3]; - const uint32_t azimuth_angle_std_u = - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_AZIMUTH_ANGLE_STD_BYTE + 3]; - const uint8_t invalid_flags_u = data[CURRENT_DETECTION_INVALID_FLAGS_BYTE]; - const uint32_t elevation_angle_u = - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_ELEVATION_ANGLE_BYTE + 3]; - const uint32_t elevation_angle_std_u = - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_ELEVATION_ANGLE_STD_BYTE + 3]; - const uint32_t range_u = (static_cast(data[CURRENT_DETECTION_RANGE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_BYTE + 3]; - const uint32_t range_std_u = - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_STD_BYTE + 3]; - const uint32_t range_rate_u = - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_RATE_BYTE + 3]; - const uint32_t range_rate_std_u = - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 2]) << 8) | - data[CURRENT_DETECTION_RANGE_RATE_STD_BYTE + 3]; - const uint8_t rcs_u = data[CURRENT_DETECTION_RCS_BYTE]; - const uint16_t measurement_id_u = - (static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE]) << 8) | - static_cast(data[CURRENT_DETECTION_MEASUREMENT_ID_BYTE + 1]); - const uint8_t positive_predictive_value_u = - data[CURRENT_DETECTION_POSITIVE_PREDICTIVE_VALUE_BYTE]; - const uint8_t classification_u = data[CURRENT_DETECTION_CLASSIFICATION_BYTE]; - const uint8_t multi_target_probability_u = data[CURRENT_DETECTION_MULT_TARGET_PROBABILITY_BYTE]; - const uint16_t object_id_u = - (static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE]) << 8) | - static_cast(data[CURRENT_DETECTION_OBJECT_ID_BYTE + 1]); - const uint8_t ambiguity_flag_u = data[CURRENT_DETECTION_AMBIGUITY_FLAG_BYTE]; - - assert(positive_predictive_value_u <= 100); - assert(classification_u <= 4 || classification_u == 255); - assert(multi_target_probability_u <= 100); - assert(ambiguity_flag_u <= 100); - - detection_msg.invalid_distance = invalid_flags_u & 0x01; - detection_msg.invalid_distance_std = invalid_flags_u & 0x02; - detection_msg.invalid_azimuth = invalid_flags_u & 0x04; - detection_msg.invalid_azimuth_std = invalid_flags_u & 0x08; - detection_msg.invalid_elevation = invalid_flags_u & 0x10; - detection_msg.invalid_elevation_std = invalid_flags_u & 0x20; - detection_msg.invalid_range_rate = invalid_flags_u & 0x40; - detection_msg.invalid_range_rate_std = invalid_flags_u & 0x80; - detection_msg.rcs = static_cast(rcs_u); - detection_msg.measurement_id = measurement_id_u; - detection_msg.positive_predictive_value = positive_predictive_value_u; - detection_msg.classification = classification_u; - detection_msg.multi_target_probability = multi_target_probability_u; - detection_msg.object_id = object_id_u; - detection_msg.ambiguity_flag = ambiguity_flag_u; - std::memcpy(&detection_msg.azimuth_angle, &azimuth_angle_u, sizeof(azimuth_angle_u)); - std::memcpy( - &detection_msg.azimuth_angle_std, &azimuth_angle_std_u, sizeof(azimuth_angle_std_u)); - std::memcpy(&detection_msg.elevation_angle, &elevation_angle_u, sizeof(elevation_angle_u)); - std::memcpy( - &detection_msg.elevation_angle_std, &elevation_angle_std_u, sizeof(elevation_angle_std_u)); - std::memcpy(&detection_msg.range, &range_u, sizeof(range_u)); - std::memcpy(&detection_msg.range_std, &range_std_u, sizeof(range_std_u)); - std::memcpy(&detection_msg.range_rate, &range_rate_u, sizeof(range_rate_u)); - std::memcpy(&detection_msg.range_rate_std, &range_rate_std_u, sizeof(range_rate_std_u)); - - assert(detection_msg.azimuth_angle >= -M_PI && detection_msg.azimuth_angle <= M_PI); - assert(detection_msg.azimuth_angle_std >= 0.f && detection_msg.azimuth_angle_std <= 1.f); - assert(detection_msg.elevation_angle >= -M_PI && detection_msg.elevation_angle <= M_PI); - assert(detection_msg.elevation_angle_std >= 0.f && detection_msg.elevation_angle_std <= 1.f); - - assert(detection_msg.range >= 0.f && detection_msg.range <= 1500.f); - assert(detection_msg.range_std >= 0.f && detection_msg.range_std <= 1.f); - assert(detection_msg.range_rate_std >= 0.f && detection_msg.range_rate_std <= 1.f); - } - - auto msg2_ptr = std::make_unique(); - auto & msg2 = *msg2_ptr; - - DetectionList detection_list; - assert(sizeof(DetectionList) == data.size()); - - std::memcpy(&detection_list, data.data(), sizeof(DetectionList)); - - msg2.header.frame_id = sensor_configuration_->frame_id; - msg2.header.stamp.nanosec = detection_list.stamp.timestamp_nanoseconds.value(); - msg2.header.stamp.sec = detection_list.stamp.timestamp_seconds.value(); - msg2.stamp_sync_status = detection_list.stamp.timestamp_sync_status; - assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); - - msg2.origin_pos.x = detection_list.origin_x_pos.value(); - msg2.origin_pos.y = detection_list.origin_y_pos.value(); - msg2.origin_pos.z = detection_list.origin_z_pos.value(); - - msg2.origin_pitch = detection_list.origin_pitch.value(); - msg2.origin_pitch_std = detection_list.origin_pitch_std.value(); - msg2.origin_yaw = detection_list.origin_yaw.value(); - msg2.origin_yaw_std = detection_list.origin_yaw_std.value(); - - msg2.ambiguity_free_velocity_min = detection_list.list_rad_vel_domain_min.value(); - msg2.ambiguity_free_velocity_max = detection_list.list_rad_vel_domain_max.value(); - - msg2.alignment_azimuth_correction = detection_list.alignment_azimuth_correction.value(); - msg2.alignment_elevation_correction = detection_list.alignment_elevation_correction.value(); - - msg2.alignment_status = detection_list.alignment_status; - - const uint32_t number_of_detections2 = detection_list.number_of_detections.value(); - msg2.detections.resize(number_of_detections2); - - assert(msg2.origin_pos.x >= -10.f && msg2.origin_pos.x <= 10.f); - assert(msg2.origin_pos.y >= -10.f && msg2.origin_pos.y <= 10.f); - assert(msg2.origin_pos.z >= -10.f && msg2.origin_pos.z <= 10.f); - assert(msg2.origin_pitch >= -M_PI && msg2.origin_pitch <= M_PI); - assert(msg2.origin_yaw >= -M_PI && msg2.origin_yaw <= M_PI); - assert(msg2.ambiguity_free_velocity_min >= -100.f && msg2.ambiguity_free_velocity_min <= 100.f); - assert(msg2.ambiguity_free_velocity_max >= -100.f && msg2.ambiguity_free_velocity_max <= 100.f); - assert(number_of_detections2 <= 800); - assert(msg2.alignment_azimuth_correction >= -M_PI && msg2.alignment_azimuth_correction <= M_PI); - assert( - msg2.alignment_elevation_correction >= -M_PI && msg2.alignment_elevation_correction <= M_PI); - - for (std::size_t detection_index = 0; detection_index < number_of_detections2; - detection_index++) { - auto & detection_msg = msg2.detections[detection_index]; auto & detection = detection_list.detections[detection_index]; assert(detection.positive_predictive_value <= 100); @@ -442,14 +181,6 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket(const std::vector { auto msg_ptr = std::make_unique(); auto & msg = *msg_ptr; + + ObjectListPacket object_list; + assert(sizeof(ObjectListPacket) == data.size()); + + std::memcpy(&object_list, data.data(), sizeof(object_list)); + msg.header.frame_id = sensor_configuration_->frame_id; - msg.header.stamp.nanosec = - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - data[OBJECT_LIST_TIMESTAMP_NANOSECONDS_BYTE + 3]; - msg.header.stamp.sec = - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - data[OBJECT_LIST_TIMESTAMP_SECONDS_BYTE + 3]; - msg.stamp_sync_status = data[OBJECT_LIST_TIMESTAMP_SYNC_STATUS_BYTE]; + msg.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); + msg.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); + msg.stamp_sync_status = object_list.stamp.timestamp_sync_status; assert(msg.stamp_sync_status >= 1 && msg.stamp_sync_status <= 3); - const uint8_t number_of_objects = data[OBJECT_LIST_NUMBER_OF_OBJECTS_BYTE]; + const uint8_t number_of_objects = object_list.number_of_objects; msg.objects.resize(number_of_objects); for (std::size_t object_index = 0; object_index < number_of_objects; object_index++) { auto & object_msg = msg.objects[object_index]; - - const int CURRENT_OBJECT_START_BYTE = - OBJECT_LIST_ARRAY_BYTE + object_index * OBJECT_STRUCT_SIZE; - const int CURRENT_OBJECT_ID_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ID_BYTE; - const int CURRENT_OBJECT_AGE_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_AGE_BYTE; - - const int CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MEASUREMENT_BYTE; - const int CURRENT_OBJECT_STATUS_MOVEMENT_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_STATUS_MOVEMENT_BYTE; - const int CURRENT_OBJECT_POSITION_REFERENCE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_REFERENCE_BYTE; - const int CURRENT_OBJECT_POSITION_X_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_BYTE; - const int CURRENT_OBJECT_POSITION_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_X_STD_BYTE; - const int CURRENT_OBJECT_POSITION_Y_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_BYTE; - const int CURRENT_OBJECT_POSITION_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Y_STD_BYTE; - const int CURRENT_OBJECT_POSITION_Z_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_BYTE; - const int CURRENT_OBJECT_POSITION_Z_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_Z_STD_BYTE; - - const int CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_POSITION_COVARIANCE_XY_BYTE; - const int CURRENT_OBJECT_ORIENTATION_BYTE = CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_BYTE; - const int CURRENT_OBJECT_ORIENTATION_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_ORIENTATION_STD_BYTE; - - const int CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_EXISTENCE_PROBABILITY_BYTE; - - const int CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_CAR_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_TRUCK_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_BICYCLE_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_ANIMAL_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_HAZARD_BYTE; - const int CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_CLASSIFICATION_UNKNOWN_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE; - const int CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE; - - const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE; - const int CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE; - - const int CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE; - const int CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE = - CURRENT_OBJECT_START_BYTE + OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE; - - const uint32_t object_id_u = (static_cast(data[CURRENT_OBJECT_ID_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ID_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ID_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ID_BYTE + 3]; - const uint16_t object_age_u = (static_cast(data[CURRENT_OBJECT_AGE_BYTE]) << 8) | - static_cast(data[CURRENT_OBJECT_AGE_BYTE + 1]); - const uint8_t status_measurement_u = data[CURRENT_OBJECT_STATUS_MEASUREMENT_BYTE]; - const uint8_t status_movement_u = data[CURRENT_OBJECT_STATUS_MOVEMENT_BYTE]; - const uint8_t position_reference_u = data[CURRENT_OBJECT_POSITION_REFERENCE_BYTE]; - - const uint32_t position_x_u = - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_X_BYTE + 3]; - const uint32_t position_x_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_X_STD_BYTE + 3]; - const uint32_t position_y_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Y_BYTE + 3]; - const uint32_t position_y_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Y_STD_BYTE + 3]; - const uint32_t position_z_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Z_BYTE + 3]; - const uint32_t position_z_std_u = - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_Z_STD_BYTE + 3]; - const uint32_t position_xy_covariance_u = - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_POSITION_COVARIANCE_XY_BYTE + 3]; - const uint32_t orientation_u = - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ORIENTATION_BYTE + 3]; - const uint32_t orientation_std_u = - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_ORIENTATION_STD_BYTE + 3]; - const uint32_t existence_probability_u = - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_EXISTENCE_PROBABILITY_BYTE + 3]; - - float position_x_f; - float position_x_std_f; - float position_y_f; - float position_y_std_f; - float position_z_f; - float position_z_std_f; - float position_xy_covariance_f; - float orientation_f; - float orientation_std_f; - float existence_probability_f; - - std::memcpy(&position_x_f, &position_x_u, sizeof(position_x_u)); - std::memcpy(&position_x_std_f, &position_x_std_u, sizeof(position_x_std_u)); - std::memcpy(&position_y_f, &position_y_u, sizeof(position_y_u)); - std::memcpy(&position_y_std_f, &position_y_std_u, sizeof(position_y_std_u)); - std::memcpy(&position_z_f, &position_z_u, sizeof(position_z_u)); - std::memcpy(&position_z_std_f, &position_z_std_u, sizeof(position_z_std_u)); - std::memcpy( - &position_xy_covariance_f, &position_xy_covariance_u, sizeof(position_xy_covariance_u)); - std::memcpy(&orientation_f, &orientation_u, sizeof(orientation_u)); - std::memcpy(&orientation_std_f, &orientation_std_u, sizeof(orientation_std_u)); - std::memcpy( - &existence_probability_f, &existence_probability_u, sizeof(existence_probability_u)); - - const uint8_t classification_car_u = data[CURRENT_OBJECT_CLASSIFICATION_CAR_BYTE]; - const uint8_t classification_truck_u = data[CURRENT_OBJECT_CLASSIFICATION_TRUCK_BYTE]; - const uint8_t classification_motorcycle_u = data[CURRENT_OBJECT_CLASSIFICATION_MOTORCYCLE_BYTE]; - const uint8_t classification_bicycle_u = data[CURRENT_OBJECT_CLASSIFICATION_BICYCLE_BYTE]; - const uint8_t classification_pedestrian_u = data[CURRENT_OBJECT_CLASSIFICATION_PEDESTRIAN_BYTE]; - const uint8_t classification_animal_u = data[CURRENT_OBJECT_CLASSIFICATION_ANIMAL_BYTE]; - const uint8_t classification_hazard_u = data[CURRENT_OBJECT_CLASSIFICATION_HAZARD_BYTE]; - const uint8_t classification_unknown_u = data[CURRENT_OBJECT_CLASSIFICATION_UNKNOWN_BYTE]; - - const uint32_t dynamics_abs_vel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_BYTE + 3]; - const uint32_t dynamics_abs_vel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_X_STD_BYTE + 3]; - const uint32_t dynamics_abs_vel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_BYTE + 3]; - const uint32_t dynamics_abs_vel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_abs_vel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_VEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_rel_vel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_BYTE + 3]; - const uint32_t dynamics_rel_vel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_X_STD_BYTE + 3]; - const uint32_t dynamics_rel_vel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_BYTE + 3]; - const uint32_t dynamics_rel_vel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_rel_vel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_VEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_abs_accel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_BYTE + 3]; - const uint32_t dynamics_abs_accel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_X_STD_BYTE + 3]; - const uint32_t dynamics_abs_accel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_BYTE + 3]; - const uint32_t dynamics_abs_accel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_abs_accel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 1]) - << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ABS_ACCEL_COVARIANCE_XY_BYTE + 3]; - - const uint32_t dynamics_rel_accel_x_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_BYTE + 3]; - const uint32_t dynamics_rel_accel_x_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_X_STD_BYTE + 3]; - const uint32_t dynamics_rel_accel_y_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_BYTE + 3]; - ; - const uint32_t dynamics_rel_accel_y_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_Y_STD_BYTE + 3]; - const uint32_t dynamics_rel_accel_covariance_xy_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 1]) - << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_REL_ACCEL_COVARIANCE_XY_BYTE + 3]; - - float dynamics_abs_vel_x_f; - float dynamics_abs_vel_x_std_f; - float dynamics_abs_vel_y_f; - float dynamics_abs_vel_y_std_f; - float dynamics_abs_vel_covariance_xy_f; - - float dynamics_rel_vel_x_f; - float dynamics_rel_vel_x_std_f; - float dynamics_rel_vel_y_f; - float dynamics_rel_vel_y_std_f; - float dynamics_rel_vel_covariance_xy_f; - - float dynamics_abs_accel_x_f; - float dynamics_abs_accel_x_std_f; - float dynamics_abs_accel_y_f; - float dynamics_abs_accel_y_std_f; - float dynamics_abs_accel_covariance_xy_f; - - float dynamics_rel_accel_x_f; - float dynamics_rel_accel_x_std_f; - float dynamics_rel_accel_y_f; - float dynamics_rel_accel_y_std_f; - float dynamics_rel_accel_covariance_xy_f; - - std::memcpy(&dynamics_abs_vel_x_f, &dynamics_abs_vel_x_u, sizeof(dynamics_abs_vel_x_u)); - std::memcpy( - &dynamics_abs_vel_x_std_f, &dynamics_abs_vel_x_std_u, sizeof(dynamics_abs_vel_x_std_u)); - std::memcpy(&dynamics_abs_vel_y_f, &dynamics_abs_vel_y_u, sizeof(dynamics_abs_vel_y_u)); - std::memcpy( - &dynamics_abs_vel_y_std_f, &dynamics_abs_vel_y_std_u, sizeof(dynamics_abs_vel_y_std_u)); - std::memcpy( - &dynamics_abs_vel_covariance_xy_f, &dynamics_abs_vel_covariance_xy_u, - sizeof(dynamics_abs_vel_covariance_xy_u)); - - std::memcpy(&dynamics_rel_vel_x_f, &dynamics_rel_vel_x_u, sizeof(dynamics_rel_vel_x_u)); - std::memcpy( - &dynamics_rel_vel_x_std_f, &dynamics_rel_vel_x_std_u, sizeof(dynamics_rel_vel_x_std_u)); - std::memcpy(&dynamics_rel_vel_y_f, &dynamics_rel_vel_y_u, sizeof(dynamics_rel_vel_y_u)); - std::memcpy( - &dynamics_rel_vel_y_std_f, &dynamics_rel_vel_y_std_u, sizeof(dynamics_rel_vel_y_std_u)); - std::memcpy( - &dynamics_rel_vel_covariance_xy_f, &dynamics_rel_vel_covariance_xy_u, - sizeof(dynamics_rel_vel_covariance_xy_u)); - - std::memcpy(&dynamics_abs_accel_x_f, &dynamics_abs_accel_x_u, sizeof(dynamics_abs_accel_x_u)); - std::memcpy( - &dynamics_abs_accel_x_std_f, &dynamics_abs_accel_x_std_u, sizeof(dynamics_abs_accel_x_std_u)); - std::memcpy(&dynamics_abs_accel_y_f, &dynamics_abs_accel_y_u, sizeof(dynamics_abs_accel_y_u)); - std::memcpy( - &dynamics_abs_accel_y_std_f, &dynamics_abs_accel_y_std_u, sizeof(dynamics_abs_accel_y_std_u)); - std::memcpy( - &dynamics_abs_accel_covariance_xy_f, &dynamics_abs_accel_covariance_xy_u, - sizeof(dynamics_abs_accel_covariance_xy_u)); - - std::memcpy(&dynamics_rel_accel_x_f, &dynamics_rel_accel_x_u, sizeof(dynamics_rel_accel_x_u)); - std::memcpy( - &dynamics_rel_accel_x_std_f, &dynamics_rel_accel_x_std_u, sizeof(dynamics_rel_accel_x_std_u)); - std::memcpy(&dynamics_rel_accel_y_f, &dynamics_rel_accel_y_u, sizeof(dynamics_rel_accel_y_u)); - std::memcpy( - &dynamics_rel_accel_y_std_f, &dynamics_rel_accel_y_std_u, sizeof(dynamics_rel_accel_y_std_u)); - std::memcpy( - &dynamics_rel_accel_covariance_xy_f, &dynamics_rel_accel_covariance_xy_u, - sizeof(dynamics_rel_accel_covariance_xy_u)); - - const uint32_t dynamics_orientation_rate_mean_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_BYTE + 3]; - const uint32_t dynamics_orientation_rate_std_u = - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_DYNAMICS_ORIENTATION_RATE_STD_BYTE + 3]; - - const uint32_t shape_length_edge_mean_u = - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_SHAPE_LENGTH_EDGE_MEAN_BYTE + 3]; - const uint32_t shape_width_edge_mean_u = - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE]) << 24) | - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 1]) << 16) | - (static_cast(data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 2]) << 8) | - data[CURRENT_OBJECT_SHAPE_WIDTH_EDGE_MEAN_BYTE + 3]; - - float dynamics_orientation_rate_mean_f; - float dynamics_orientation_rate_std_f; - - float shape_length_edge_mean_f; - float shape_width_edge_mean_f; - - std::memcpy( - &dynamics_orientation_rate_mean_f, &dynamics_orientation_rate_mean_u, - sizeof(dynamics_orientation_rate_mean_u)); - std::memcpy( - &dynamics_orientation_rate_std_f, &dynamics_orientation_rate_std_u, - sizeof(dynamics_orientation_rate_std_u)); - std::memcpy( - &shape_length_edge_mean_f, &shape_length_edge_mean_u, sizeof(shape_length_edge_mean_u)); - std::memcpy( - &shape_width_edge_mean_f, &shape_width_edge_mean_u, sizeof(shape_width_edge_mean_u)); - - assert(status_measurement_u <= 2 || status_measurement_u == 255); - assert(status_movement_u <= 1 || status_movement_u == 255); - assert(position_reference_u <= 7 || position_reference_u == 255); - - assert(position_x_f >= -1600.f && position_x_f <= 1600.f); - assert(position_x_std_f >= 0.f); - assert(position_y_f >= -1600.f && position_y_f <= 1600.f); - assert(position_y_std_f >= 0.f); - assert(position_z_f >= -1600.f && position_z_f <= 1600.f); - assert(position_z_std_f >= 0.f); - assert(orientation_f >= -M_PI && orientation_f <= M_PI); - assert(orientation_std_f >= 0.f); - - assert(classification_car_u <= 100); - assert(classification_truck_u <= 100); - assert(classification_motorcycle_u <= 100); - assert(classification_bicycle_u <= 100); - assert(classification_pedestrian_u <= 100); - assert(classification_animal_u <= 100); - assert(classification_hazard_u <= 100); - assert(classification_unknown_u <= 100); - - assert(dynamics_abs_vel_x_std_f >= 0.f); - assert(dynamics_abs_vel_y_std_f >= 0.f); - - assert(dynamics_rel_vel_x_std_f >= 0.f); - assert(dynamics_rel_vel_y_std_f >= 0.f); - - assert(dynamics_abs_accel_x_std_f >= 0.f); - assert(dynamics_abs_accel_y_std_f >= 0.f); - - assert(dynamics_rel_accel_x_std_f >= 0.f); - assert(dynamics_rel_accel_y_std_f >= 0.f); - - object_msg.object_id = object_id_u; - object_msg.age = object_age_u; - object_msg.status_measurement = status_measurement_u; - object_msg.status_movement = status_movement_u; - object_msg.position_reference = position_reference_u; - - object_msg.position.x = static_cast(position_x_f); - object_msg.position.y = static_cast(position_y_f); - object_msg.position.y = static_cast(position_z_f); - - object_msg.position_std.x = static_cast(position_x_std_f); - object_msg.position_std.y = static_cast(position_y_std_f); - object_msg.position_std.z = static_cast(position_z_std_f); - - object_msg.position_covariance_xy = position_xy_covariance_f; - - object_msg.orientation = orientation_f; - object_msg.orientation_std = orientation_std_f; - - object_msg.existence_probability = existence_probability_f; - object_msg.classification_car = classification_car_u; - object_msg.classification_truck = classification_truck_u; - object_msg.classification_motorcycle = classification_motorcycle_u; - object_msg.classification_bicycle = classification_bicycle_u; - object_msg.classification_pedestrian = classification_pedestrian_u; - object_msg.classification_animal = classification_animal_u; - object_msg.classification_hazard = classification_hazard_u; - object_msg.classification_unknown = classification_unknown_u; - - object_msg.absolute_velocity.x = static_cast(dynamics_abs_vel_x_f); - object_msg.absolute_velocity.y = static_cast(dynamics_abs_vel_y_f); - object_msg.absolute_velocity_std.x = static_cast(dynamics_abs_vel_x_std_f); - object_msg.absolute_velocity_std.y = static_cast(dynamics_abs_vel_y_std_f); - object_msg.absolute_velocity_covariance_xy = dynamics_abs_vel_covariance_xy_f; - - object_msg.relative_velocity.x = static_cast(dynamics_rel_vel_x_f); - object_msg.relative_velocity.y = static_cast(dynamics_rel_vel_y_f); - object_msg.relative_velocity_std.x = static_cast(dynamics_rel_vel_x_std_f); - object_msg.relative_velocity_std.y = static_cast(dynamics_rel_vel_y_std_f); - object_msg.relative_velocity_covariance_xy = dynamics_rel_vel_covariance_xy_f; - - object_msg.absolute_acceleration.x = static_cast(dynamics_abs_accel_x_f); - object_msg.absolute_acceleration.y = static_cast(dynamics_abs_accel_y_f); - object_msg.absolute_acceleration_std.x = static_cast(dynamics_abs_accel_x_std_f); - object_msg.absolute_acceleration_std.y = static_cast(dynamics_abs_accel_y_std_f); - object_msg.absolute_acceleration_covariance_xy = dynamics_abs_accel_covariance_xy_f; - - object_msg.relative_velocity.x = dynamics_rel_accel_x_f; - object_msg.relative_velocity.y = dynamics_rel_accel_y_f; - object_msg.relative_velocity_std.x = dynamics_rel_accel_x_std_f; - object_msg.relative_velocity_std.y = dynamics_rel_accel_y_std_f; - object_msg.relative_velocity_covariance_xy = dynamics_rel_accel_covariance_xy_f; - - object_msg.orientation_rate_mean = dynamics_orientation_rate_mean_f; - object_msg.orientation_rate_std = dynamics_orientation_rate_std_f; - - object_msg.shape_length_edge_mean = shape_length_edge_mean_f; - object_msg.shape_width_edge_mean = shape_width_edge_mean_f; - } - - auto msg2_ptr = std::make_unique(); - auto & msg2 = *msg2_ptr; - - ObjectList object_list; - assert(sizeof(ObjectList) == data.size()); - - std::memcpy(&object_list, data.data(), sizeof(object_list)); - - msg2.header.frame_id = sensor_configuration_->frame_id; - msg2.header.stamp.nanosec = object_list.stamp.timestamp_nanoseconds.value(); - msg2.header.stamp.sec = object_list.stamp.timestamp_seconds.value(); - msg2.stamp_sync_status = object_list.stamp.timestamp_sync_status; - assert(msg2.stamp_sync_status >= 1 && msg2.stamp_sync_status <= 3); - - const uint8_t number_of_objects2 = object_list.number_of_objects; - - msg2.objects.resize(number_of_objects2); - - for (std::size_t object_index = 0; object_index < number_of_objects2; object_index++) { - auto & object_msg = msg2.objects[object_index]; - const Object & object = object_list.objects[object_index]; + const ObjectPacket & object = object_list.objects[object_index]; assert(object.status_measurement <= 2 || object.status_measurement == 255); assert(object.status_movement <= 1 || object.status_movement == 255); @@ -1099,14 +308,6 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket(const std::vector object_msg.shape_width_edge_mean = object.shape_width_edge_mean.value(); } - assert(number_of_objects2 == number_of_objects); - - for (std::size_t i = 0; i < number_of_objects; i++) { - assert(msg.objects[i] == msg2.objects[i]); - } - - assert(msg == msg2); - object_list_callback_(std::move(msg_ptr)); return true; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 94424b5d7..68371684e 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -65,18 +65,9 @@ class ContinentalARS548HwInterface : NebulaHwInterfaceBase std::mutex sensor_status_mutex_; - struct FilterStatus - { - uint8_t active; - uint8_t filter_id; - uint8_t min_value; - uint8_t max_value; - }; - - FilterStatus detection_filters_status_[DETECTION_FILTER_PROPERTIES_NUM]; - FilterStatus object_filters_status_[OBJECT_FILTER_PROPERTIES_NUM]; - - ContinentalARS548Status radar_status_; + SensorStatusPacket sensor_status_packet_{}; + FilterStatusPacket filter_status_{}; + ContinentalARS548Status radar_status_{}; std::shared_ptr parent_node_logger; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp index 407632844..ec1c06190 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_types.hpp @@ -52,13 +52,13 @@ struct ContinentalARS548Status std::string sensor_ip_address0; std::string sensor_ip_address1; uint8_t configuration_counter; - std::string status_longitudinal_velocity; - std::string status_longitudinal_acceleration; - std::string status_lateral_acceleration; - std::string status_yaw_rate; - std::string status_steering_angle; - std::string status_driving_direction; - std::string characteristic_speed; + std::string longitudinal_velocity_status; + std::string longitudinal_acceleration_status; + std::string lateral_acceleration_status; + std::string yaw_rate_status; + std::string steering_angle_status; + std::string driving_direction_status; + std::string characteristic_speed_status; std::string radar_status; std::string voltage_status; std::string temperature_status; @@ -122,19 +122,19 @@ struct ContinentalARS548Status os << ", "; os << "configuration_counter: " << arg.configuration_counter; os << ", "; - os << "status_longitudinal_velocity: " << arg.status_longitudinal_velocity; + os << "status_longitudinal_velocity: " << arg.longitudinal_velocity_status; os << ", "; - os << "status_longitudinal_acceleration: " << arg.status_longitudinal_acceleration; + os << "status_longitudinal_acceleration: " << arg.longitudinal_acceleration_status; os << ", "; - os << "status_lateral_acceleration: " << arg.status_lateral_acceleration; + os << "status_lateral_acceleration: " << arg.lateral_acceleration_status; os << ", "; - os << "status_yaw_rate: " << arg.status_yaw_rate; + os << "status_yaw_rate: " << arg.yaw_rate_status; os << ", "; - os << "status_steering_angle: " << arg.status_steering_angle; + os << "status_steering_angle: " << arg.steering_angle_status; os << ", "; - os << "status_driving_direction: " << arg.status_driving_direction; + os << "status_driving_direction: " << arg.driving_direction_status; os << ", "; - os << "characteristic_speed: " << arg.characteristic_speed; + os << "characteristic_speed: " << arg.characteristic_speed_status; os << ", "; os << "radar_status: " << arg.radar_status; os << ", "; diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 519724718..22fdeaa77 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -97,69 +97,47 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallbackWithSender( } void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector & buffer) { - /*constexpr int DETECTION_LIST_METHOD_ID = 336; - constexpr int OBJECT_LIST_METHOD_ID = 329; - constexpr int SENSOR_STATUS_METHOD_ID = 380; - constexpr int FILTER_STATUS_METHOD_ID = 396; - - constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; - constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; - constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; - constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; - - constexpr int DETECTION_LIST_PDU_LENGTH = 35328; - constexpr int OBJECT_LIST_PDU_LENGTH = 9393; - constexpr int SENSOR_STATUS_PDU_LENGTH = 76; - constexpr int FILTER_STATUS_PDU_LENGTH = 322; - - */ - - if (buffer.size() < sizeof(Header)) { + if (buffer.size() < sizeof(HeaderPacket)) { PrintError("Unrecognized packet. Too short"); return; } - Header header{}; - std::memcpy(&header, buffer.data(), sizeof(Header)); + HeaderPacket header_packet{}; + std::memcpy(&header_packet, buffer.data(), sizeof(HeaderPacket)); - const uint16_t service_id = - (static_cast(buffer[SERVICE_ID_BYTE]) << 8) | buffer[SERVICE_ID_BYTE + 1]; - const uint16_t method_id = - (static_cast(buffer[METHOD_ID_BYTE]) << 8) | buffer[METHOD_ID_BYTE + 1]; - const uint32_t length = (static_cast(buffer[LENGTH_BYTE]) << 24) | - (static_cast(buffer[LENGTH_BYTE + 1]) << 16) | - (static_cast(buffer[LENGTH_BYTE + 2]) << 8) | - buffer[LENGTH_BYTE + 3]; - - assert(header.service_id.value() == service_id); - assert(header.method_id.value() == method_id); - assert(header.length.value() == length); - - if (service_id != 0) { + if (header_packet.service_id.value() != 0) { PrintError("Invalid service id"); return; - } else if (method_id == SENSOR_STATUS_METHOD_ID) { - if (buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || length != SENSOR_STATUS_PDU_LENGTH) { + } else if (header_packet.method_id.value() == SENSOR_STATUS_METHOD_ID) { + if ( + buffer.size() != SENSOR_STATUS_UDP_PAYLOAD || + header_packet.length.value() != SENSOR_STATUS_PDU_LENGTH) { PrintError("SensorStatus message with invalid size"); return; } ProcessSensorStatusPacket(buffer); - } else if (method_id == FILTER_STATUS_METHOD_ID) { - if (buffer.size() != FILTER_STATUS_UDP_PAYLOAD || length != FILTER_STATUS_PDU_LENGTH) { + } else if (header_packet.method_id.value() == FILTER_STATUS_METHOD_ID) { + if ( + buffer.size() != FILTER_STATUS_UDP_PAYLOAD || + header_packet.length.value() != FILTER_STATUS_PDU_LENGTH) { PrintError("FilterStatus message with invalid size"); return; } ProcessFilterStatusPacket(buffer); - } else if (method_id == DETECTION_LIST_METHOD_ID) { - if (buffer.size() != DETECTION_LIST_UDP_PAYLOAD || length != DETECTION_LIST_PDU_LENGTH) { + } else if (header_packet.method_id.value() == DETECTION_LIST_METHOD_ID) { + if ( + buffer.size() != DETECTION_LIST_UDP_PAYLOAD || + header_packet.length.value() != DETECTION_LIST_PDU_LENGTH) { PrintError("DetectionList message with invalid size"); return; } ProcessDataPacket(buffer); - } else if (method_id == OBJECT_LIST_METHOD_ID) { - if (buffer.size() != OBJECT_LIST_UDP_PAYLOAD || length != OBJECT_LIST_PDU_LENGTH) { + } else if (header_packet.method_id.value() == OBJECT_LIST_METHOD_ID) { + if ( + buffer.size() != OBJECT_LIST_UDP_PAYLOAD || + header_packet.length.value() != OBJECT_LIST_PDU_LENGTH) { PrintError("ObjectList message with invalid size"); return; } @@ -170,273 +148,161 @@ void ContinentalARS548HwInterface::ReceiveCloudPacketCallback(const std::vector< void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { - radar_status_.timestamp_nanoseconds = - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - buffer[STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; - radar_status_.timestamp_seconds = - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - buffer[STATUS_TIMESTAMP_SECONDS_BYTE + 3]; - - const uint8_t & sync_status = buffer[STATUS_SYNC_STATUS_BYTE]; - - SensorStatus sensor_status{}; - std::memcpy(&sensor_status, buffer.data(), sizeof(SensorStatus)); - assert(sensor_status.stamp.timestamp_nanoseconds.value() == radar_status_.timestamp_nanoseconds); - assert(sensor_status.stamp.timestamp_seconds.value() == radar_status_.timestamp_seconds); - assert(sync_status == sensor_status.stamp.timestamp_sync_status); - - if (sync_status == 1) { + std::memcpy(&sensor_status_packet_, buffer.data(), sizeof(SensorStatusPacket)); + + radar_status_.timestamp_nanoseconds = sensor_status_packet_.stamp.timestamp_nanoseconds.value(); + radar_status_.timestamp_seconds = sensor_status_packet_.stamp.timestamp_seconds.value(); + + if (sensor_status_packet_.stamp.timestamp_sync_status == 1) { radar_status_.timestamp_sync_status = "SYNC_OK"; - } else if (sync_status == 2) { + } else if (sensor_status_packet_.stamp.timestamp_sync_status == 2) { radar_status_.timestamp_sync_status = "NEVER_SYNC"; - } else if (sync_status == 3) { + } else if (sensor_status_packet_.stamp.timestamp_sync_status == 3) { radar_status_.timestamp_sync_status = "SYNC_LOST"; } else { radar_status_.timestamp_sync_status = "INVALID_VALUE"; } - radar_status_.sw_version_major = buffer[STATUS_SW_VERSION_MAJOR_BYTE]; - radar_status_.sw_version_minor = buffer[STATUS_SW_VERSION_MINOR_BYTE]; - radar_status_.sw_version_patch = buffer[STATUS_SW_VERSION_PATCH_BYTE]; - - assert(sensor_status.sw_version_major == radar_status_.sw_version_major); - assert(sensor_status.sw_version_minor == radar_status_.sw_version_minor); - assert(sensor_status.sw_version_patch == radar_status_.sw_version_patch); - - const uint32_t status_longitudinal_u = - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LONGITUDINAL_BYTE + 2]) << 8) | - buffer[STATUS_LONGITUDINAL_BYTE + 3]; - const uint32_t status_lateral_u = (static_cast(buffer[STATUS_LATERAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_LATERAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LATERAL_BYTE + 2]) << 8) | - buffer[STATUS_LATERAL_BYTE + 3]; - const uint32_t status_vertical_u = - (static_cast(buffer[STATUS_VERTICAL_BYTE]) << 24) | - (static_cast(buffer[STATUS_VERTICAL_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_VERTICAL_BYTE + 2]) << 8) | - buffer[STATUS_VERTICAL_BYTE + 3]; - const uint32_t status_yaw_u = (static_cast(buffer[STATUS_YAW_BYTE]) << 24) | - (static_cast(buffer[STATUS_YAW_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_YAW_BYTE + 2]) << 8) | - buffer[STATUS_YAW_BYTE + 3]; - const uint32_t status_pitch_u = (static_cast(buffer[STATUS_PITCH_BYTE]) << 24) | - (static_cast(buffer[STATUS_PITCH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_PITCH_BYTE + 2]) << 8) | - buffer[STATUS_PITCH_BYTE + 3]; - const uint8_t & plug_orientation = buffer[STATUS_PLUG_ORIENTATION_BYTE]; - radar_status_.plug_orientation = plug_orientation == 0 ? "PLUG_RIGHT" - : plug_orientation == 1 ? "PLUG_LEFT" - : "INVALID_VALUE"; - const uint32_t status_length_u = (static_cast(buffer[STATUS_LENGTH_BYTE]) << 24) | - (static_cast(buffer[STATUS_LENGTH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_LENGTH_BYTE + 2]) << 8) | - buffer[STATUS_LENGTH_BYTE + 3]; - const uint32_t status_width_u = (static_cast(buffer[STATUS_WIDTH_BYTE]) << 24) | - (static_cast(buffer[STATUS_WIDTH_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_WIDTH_BYTE + 2]) << 8) | - buffer[STATUS_WIDTH_BYTE + 3]; - const uint32_t status_height_u = (static_cast(buffer[STATUS_HEIGHT_BYTE]) << 24) | - (static_cast(buffer[STATUS_HEIGHT_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_HEIGHT_BYTE + 2]) << 8) | - buffer[STATUS_HEIGHT_BYTE + 3]; - const uint32_t status_wheel_base_u = - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE]) << 24) | - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 1]) << 16) | - (static_cast(buffer[STATUS_WHEEL_BASE_BYTE + 2]) << 8) | - buffer[STATUS_WHEEL_BASE_BYTE + 3]; - radar_status_.max_distance = (static_cast(buffer[STATUS_MAXIMUM_DISTANCE_BYTE]) << 8) | - buffer[STATUS_MAXIMUM_DISTANCE_BYTE + 1]; - - std::memcpy(&radar_status_.longitudinal, &status_longitudinal_u, sizeof(status_longitudinal_u)); - std::memcpy(&radar_status_.lateral, &status_lateral_u, sizeof(status_lateral_u)); - std::memcpy(&radar_status_.vertical, &status_vertical_u, sizeof(status_vertical_u)); - std::memcpy(&radar_status_.yaw, &status_yaw_u, sizeof(status_yaw_u)); - - std::memcpy(&radar_status_.pitch, &status_pitch_u, sizeof(status_pitch_u)); - std::memcpy(&radar_status_.length, &status_length_u, sizeof(status_length_u)); - std::memcpy(&radar_status_.width, &status_width_u, sizeof(status_width_u)); - std::memcpy(&radar_status_.height, &status_height_u, sizeof(status_height_u)); - std::memcpy(&radar_status_.wheel_base, &status_wheel_base_u, sizeof(status_wheel_base_u)); - - const uint8_t & frequency_slot = buffer[STATUS_FREQUENCY_SLOT_BYTE]; - - if (frequency_slot == 0) { + radar_status_.sw_version_major = sensor_status_packet_.sw_version_major; + radar_status_.sw_version_minor = sensor_status_packet_.sw_version_minor; + radar_status_.sw_version_patch = sensor_status_packet_.sw_version_patch; + + radar_status_.plug_orientation = sensor_status_packet_.status.plug_orientation == 0 ? "PLUG_RIGHT" + : sensor_status_packet_.status.plug_orientation == 1 + ? "PLUG_LEFT" + : "INVALID_VALUE"; + + radar_status_.max_distance = sensor_status_packet_.status.maximum_distance.value(); + + radar_status_.longitudinal = sensor_status_packet_.status.longitudinal.value(); + radar_status_.lateral = sensor_status_packet_.status.lateral.value(); + radar_status_.vertical = sensor_status_packet_.status.vertical.value(); + radar_status_.yaw = sensor_status_packet_.status.yaw.value(); + + radar_status_.pitch = sensor_status_packet_.status.pitch.value(); + ; + radar_status_.length = sensor_status_packet_.status.length.value(); + ; + radar_status_.width = sensor_status_packet_.status.width.value(); + radar_status_.height = sensor_status_packet_.status.height.value(); + radar_status_.wheel_base = sensor_status_packet_.status.wheelbase.value(); + + if (sensor_status_packet_.status.frequency_slot == 0) { radar_status_.frequency_slot = "0:Low (76.23 GHz)"; - } else if (frequency_slot == 1) { + } else if (sensor_status_packet_.status.frequency_slot == 1) { radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; - } else if (frequency_slot == 2) { + } else if (sensor_status_packet_.status.frequency_slot == 2) { radar_status_.frequency_slot = "2:High (76.73 GHz)"; } else { radar_status_.frequency_slot = "INVALID VALUE"; } - radar_status_.cycle_time = buffer[STATUS_CYCLE_TIME_BYTE]; - radar_status_.time_slot = buffer[STATUS_TIME_SLOT_BYTE]; - - const uint8_t & hcc = buffer[STATUS_HCC_BYTE]; + radar_status_.cycle_time = sensor_status_packet_.status.cycle_time; + radar_status_.time_slot = sensor_status_packet_.status.time_slot; - radar_status_.hcc = hcc == 1 ? "Worldwide" - : hcc == 2 ? "Japan" - : ("INVALID VALUE=" + std::to_string(hcc)); + radar_status_.hcc = sensor_status_packet_.status.hcc == 1 ? "Worldwide" + : sensor_status_packet_.status.hcc == 2 + ? "Japan" + : ("INVALID VALUE=" + std::to_string(sensor_status_packet_.status.hcc)); - const uint8_t & power_save_standstill = buffer[STATUS_POWER_SAVING_STANDSTILL_BYTE]; - radar_status_.power_save_standstill = power_save_standstill == 0 ? "Off" - : power_save_standstill == 1 ? "On" - : "INVALID VALUE"; - - const uint8_t status_sensor_ip_address00 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE]; - const uint8_t status_sensor_ip_address01 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 1]; - const uint8_t status_sensor_ip_address02 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 2]; - const uint8_t status_sensor_ip_address03 = buffer[STATUS_SENSOR_IP_ADDRESS0_BYTE + 3]; - const uint8_t status_sensor_ip_address10 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE]; - const uint8_t status_sensor_ip_address11 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 1]; - const uint8_t status_sensor_ip_address12 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 2]; - const uint8_t status_sensor_ip_address13 = buffer[STATUS_SENSOR_IP_ADDRESS1_BYTE + 3]; + radar_status_.power_save_standstill = + sensor_status_packet_.status.powersave_standstill == 0 ? "Off" + : sensor_status_packet_.status.powersave_standstill == 1 ? "On" + : "INVALID VALUE"; std::stringstream ss0, ss1; - ss0 << std::to_string(status_sensor_ip_address00) << "." - << std::to_string(status_sensor_ip_address01) << "." - << std::to_string(status_sensor_ip_address02) << "." - << std::to_string(status_sensor_ip_address03); + ss0 << std::to_string(sensor_status_packet_.status.sensor_ip_address00) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address01) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address02) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address03); radar_status_.sensor_ip_address0 = ss0.str(); - ss1 << std::to_string(status_sensor_ip_address10) << "." - << std::to_string(status_sensor_ip_address11) << "." - << std::to_string(status_sensor_ip_address12) << "." - << std::to_string(status_sensor_ip_address13); + ss1 << std::to_string(sensor_status_packet_.status.sensor_ip_address10) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address11) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address12) << "." + << std::to_string(sensor_status_packet_.status.sensor_ip_address13); radar_status_.sensor_ip_address1 = ss1.str(); - assert(sensor_status.status.plug_orientation == plug_orientation); - assert(sensor_status.status.maximum_distance.value() == radar_status_.max_distance); - assert(sensor_status.status.longitudinal.value() == radar_status_.longitudinal); - assert(sensor_status.status.lateral.value() == radar_status_.lateral); - assert(sensor_status.status.vertical.value() == radar_status_.vertical); - assert(sensor_status.status.yaw.value() == radar_status_.yaw); - assert(sensor_status.status.pitch.value() == radar_status_.pitch); - assert(sensor_status.status.length.value() == radar_status_.length); - assert(sensor_status.status.width.value() == radar_status_.width); - assert(sensor_status.status.height.value() == radar_status_.height); - assert(sensor_status.status.wheelbase.value() == radar_status_.wheel_base); - assert(sensor_status.status.frequency_slot == frequency_slot); - assert(sensor_status.status.cycle_time == radar_status_.cycle_time); - assert(sensor_status.status.time_slot == radar_status_.time_slot); - assert(sensor_status.status.hcc == hcc); - assert(sensor_status.status.powersave_standstill == power_save_standstill); - assert(sensor_status.status.sensor_ip_address00 == status_sensor_ip_address00); - assert(sensor_status.status.sensor_ip_address01 == status_sensor_ip_address01); - assert(sensor_status.status.sensor_ip_address02 == status_sensor_ip_address02); - assert(sensor_status.status.sensor_ip_address03 == status_sensor_ip_address03); - - assert(sensor_status.status.sensor_ip_address10 == status_sensor_ip_address10); - assert(sensor_status.status.sensor_ip_address11 == status_sensor_ip_address11); - assert(sensor_status.status.sensor_ip_address12 == status_sensor_ip_address12); - assert(sensor_status.status.sensor_ip_address13 == status_sensor_ip_address13); - - radar_status_.configuration_counter = buffer[STATUS_CONFIGURATION_COUNTER_BYTE]; - assert(sensor_status.configuration_counter == radar_status_.configuration_counter); - - const uint8_t & status_longitudinal_velocity = buffer[STATUS_LONGITUDINAL_VELOCITY_BYTE]; - assert(sensor_status.status_longitudinal_velocity == status_longitudinal_velocity); - radar_status_.status_longitudinal_velocity = status_longitudinal_velocity == 0 ? "VDY_OK" - : status_longitudinal_velocity == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_longitudinal_acceleration = buffer[STATUS_LONGITUDINAL_ACCELERATION_BYTE]; - assert(sensor_status.status_longitudinal_acceleration == status_longitudinal_acceleration); - radar_status_.status_longitudinal_acceleration = status_longitudinal_acceleration == 0 ? "VDY_OK" - : status_longitudinal_acceleration == 1 - ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_lateral_acceleration = buffer[STATUS_LATERAL_ACCELERATION_BYTE]; - assert(sensor_status.status_lateral_acceleration == status_lateral_acceleration); - radar_status_.status_lateral_acceleration = status_lateral_acceleration == 0 ? "VDY_OK" - : status_lateral_acceleration == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_yaw_rate = buffer[STATUS_YAW_RATE_BYTE]; - assert(sensor_status.status_yaw_rate == status_yaw_rate); - radar_status_.status_yaw_rate = status_yaw_rate == 0 ? "VDY_OK" - : status_yaw_rate == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_steering_angle = buffer[STATUS_STEERING_ANGLE_BYTE]; - assert(sensor_status.status_steering_angle == status_steering_angle); - radar_status_.status_steering_angle = status_steering_angle == 0 ? "VDY_OK" - : status_steering_angle == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & status_driving_direction = buffer[STATUS_DRIVING_DIRECTION_BYTE]; - assert(sensor_status.status_driving_direction == status_driving_direction); - radar_status_.status_driving_direction = status_driving_direction == 0 ? "VDY_OK" - : status_driving_direction == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & characteristic_speed = buffer[STATUS_CHARACTERISTIC_SPEED_BYTE]; - assert(sensor_status.status_characteristic_speed == characteristic_speed); - radar_status_.characteristic_speed = characteristic_speed == 0 ? "VDY_OK" - : characteristic_speed == 1 ? "VDY_NOTOK" - : "INVALID VALUE"; - - const uint8_t & radar_status = buffer[STATUS_RADAR_STATUS_BYTE]; - assert(sensor_status.status_radar_status == radar_status); - if (radar_status == 0) { + radar_status_.configuration_counter = sensor_status_packet_.configuration_counter; + + radar_status_.longitudinal_velocity_status = + sensor_status_packet_.longitudinal_velocity_status == 0 ? "VDY_OK" + : sensor_status_packet_.longitudinal_velocity_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.longitudinal_acceleration_status = + sensor_status_packet_.longitudinal_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet_.longitudinal_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.lateral_acceleration_status = + sensor_status_packet_.lateral_acceleration_status == 0 ? "VDY_OK" + : sensor_status_packet_.lateral_acceleration_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.yaw_rate_status = sensor_status_packet_.yaw_rate_status == 0 ? "VDY_OK" + : sensor_status_packet_.yaw_rate_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.steering_angle_status = sensor_status_packet_.steering_angle_status == 0 ? "VDY_OK" + : sensor_status_packet_.steering_angle_status == 1 + ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.driving_direction_status = + sensor_status_packet_.driving_direction_status == 0 ? "VDY_OK" + : sensor_status_packet_.driving_direction_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + radar_status_.characteristic_speed_status = + sensor_status_packet_.characteristic_speed_status == 0 ? "VDY_OK" + : sensor_status_packet_.characteristic_speed_status == 1 ? "VDY_NOTOK" + : "INVALID VALUE"; + + if (sensor_status_packet_.radar_status == 0) { radar_status_.radar_status = "STATE_INIT"; - } else if (radar_status == 1) { + } else if (sensor_status_packet_.radar_status == 1) { radar_status_.radar_status = "STATE_OK"; - } else if (radar_status == 2) { + } else if (sensor_status_packet_.radar_status == 2) { radar_status_.radar_status = "STATE_INVALID"; } else { radar_status_.radar_status = "INVALID VALUE"; } - const uint8_t & voltage_status = buffer[STATUS_VOLTAGE_STATUS_BYTE]; - assert(sensor_status.status_voltage_status == voltage_status); - if (voltage_status == 0) { + if (sensor_status_packet_.voltage_status == 0) { radar_status_.voltage_status = "Ok"; } - if (voltage_status & 0x01) { + if (sensor_status_packet_.voltage_status & 0x01) { radar_status_.voltage_status += "Current under voltage"; } - if (voltage_status & 0x02) { + if (sensor_status_packet_.voltage_status & 0x02) { radar_status_.voltage_status = "Past under voltage"; } - if (voltage_status & 0x03) { + if (sensor_status_packet_.voltage_status & 0x03) { radar_status_.voltage_status = "Current over voltage"; } - if (voltage_status & 0x04) { + if (sensor_status_packet_.voltage_status & 0x04) { radar_status_.voltage_status = "Past over voltage"; } - const uint8_t & temperature_status = buffer[STATUS_TEMPERATURE_STATUS_BYTE]; - assert(sensor_status.status_temperature_status == temperature_status); - if (temperature_status == 0) { + if (sensor_status_packet_.temperature_status == 0) { radar_status_.temperature_status = "Ok"; } - if (temperature_status & 0x01) { + if (sensor_status_packet_.temperature_status & 0x01) { radar_status_.temperature_status += "Current under temperature"; } - if (temperature_status & 0x02) { + if (sensor_status_packet_.temperature_status & 0x02) { radar_status_.temperature_status += "Past under temperature"; } - if (temperature_status & 0x03) { + if (sensor_status_packet_.temperature_status & 0x03) { radar_status_.temperature_status += "Current over temperature"; } - if (temperature_status & 0x04) { + if (sensor_status_packet_.temperature_status & 0x04) { radar_status_.temperature_status += "Past over temperature"; } - const uint8_t & blockage_status = buffer[STATUS_BLOCKAGE_STATUS_BYTE]; - assert(sensor_status.status_blockage_status == blockage_status); - const uint8_t & blockage_status0 = blockage_status & 0x0f; - const uint8_t & blockage_status1 = (blockage_status & 0xf0) >> 4; + const uint8_t & blockage_status0 = sensor_status_packet_.blockage_status & 0x0f; + const uint8_t & blockage_status1 = (sensor_status_packet_.blockage_status & 0xf0) >> 4; if (blockage_status0 == 0) { radar_status_.blockage_status = "Blind"; @@ -465,73 +331,8 @@ void ContinentalARS548HwInterface::ProcessSensorStatusPacket(const std::vector & buffer) { - // assert(false); // it seems 548 does not have this anymore.... - // Unused available data - // constexpr int FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE = 8; - // constexpr int FILTER_STATUS_TIMESTAMP_SECONDS_BYTE = 12; - // constexpr int FILTER_STATUS_SYNC_STATUS_BYTE = 16; - // constexpr int FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE = 17; - // constexpr int FILTER_STATUS_DETECTION_SORT_INDEX_BYTE = 18; - // constexpr int FILTER_STATUS_OBJECT_SORT_INDEX_BYTE = 19; - constexpr int FILTER_STATUS_DETECTION_FILTER_BYTE = 20; - constexpr int FILTER_STATUS_OBJECT_FILTER_BYTE = 90; - - // Unused available data - // const uint32_t filter_status_timestamp_nanoseconds = - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE]) << 24) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 1]) << 16) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 2]) << 8) | - // buffer[FILTER_STATUS_TIMESTAMP_NANOSECONDS_BYTE + 3]; const uint32_t - // filter_status_timestamp_seconds = - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE]) << 24) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 1]) << 16) | - // (static_cast(buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 2]) << 8) | - // buffer[FILTER_STATUS_TIMESTAMP_SECONDS_BYTE + 3]; const uint8_t filter_status_sync_status = - // buffer[FILTER_STATUS_SYNC_STATUS_BYTE]; const uint8_t - // filter_status_filter_configuration_counter = - // buffer[FILTER_STATUS_FILTER_CONFIGURATION_COUNTER_BYTE]; const uint8_t - // filter_status_detection_sort_index = buffer[FILTER_STATUS_DETECTION_SORT_INDEX_BYTE]; const - // uint8_t filter_status_object_sort_index = buffer[FILTER_STATUS_OBJECT_SORT_INDEX_BYTE]; - - std::size_t byte_index = FILTER_STATUS_DETECTION_FILTER_BYTE; - for (int property_index = 1; property_index <= 7; property_index++) { - FilterStatus filter_status; - filter_status.active = buffer[byte_index++]; - assert(buffer[byte_index] == property_index); - filter_status.filter_id = buffer[byte_index++]; - filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - - detection_filters_status_[property_index - 1] = filter_status; - } - - byte_index = FILTER_STATUS_OBJECT_FILTER_BYTE; - for (int property_index = 1; property_index <= 24; property_index++) { - FilterStatus filter_status; - filter_status.active = buffer[byte_index++]; - assert(buffer[byte_index] == property_index); - filter_status.filter_id = buffer[byte_index++]; - filter_status.min_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - filter_status.max_value = (static_cast(buffer[byte_index]) << 24) | - (static_cast(buffer[byte_index + 1]) << 16) | - (static_cast(buffer[byte_index + 2]) << 8) | - buffer[byte_index + 3]; - byte_index += 4; - - object_filters_status_[property_index - 1] = filter_status; - } + assert(buffer.size() == sizeof(FilterStatusPacket)); + std::memcpy(&filter_status_, buffer.data(), sizeof(FilterStatusPacket)); } void ContinentalARS548HwInterface::ProcessDataPacket(const std::vector & buffer) @@ -572,14 +373,6 @@ Status ContinentalARS548HwInterface::SetSensorMounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation) { - constexpr int CONFIGURATION_LONGITUDINAL_BYTE = 8; - constexpr int CONFIGURATION_LATERAL_BYTE = 12; - constexpr int CONFIGURATION_VERTICAL_BYTE = 16; - constexpr int CONFIGURATION_YAW_BYTE = 20; - constexpr int CONFIGURATION_PITCH_BYTE = 24; - constexpr int CONFIGURATION_PLUG_ORIENTATION_BYTE = 28; - constexpr int CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE = 60; - if ( longitudinal_autosar < -100.f || longitudinal_autosar > 100.f || lateral_autosar < -100.f || lateral_autosar > 100.f || vertical_autosar < 0.01f || vertical_autosar > 10.f || @@ -589,69 +382,20 @@ Status ContinentalARS548HwInterface::SetSensorMounting( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - uint8_t longitudinal_autosar_bytes[4]; - uint8_t lateral_autosar_bytes[4]; - uint8_t vertical_autosar_bytes[4]; - uint8_t yaw_autosar_bytes[4]; - uint8_t pitch_autosar_bytes[4]; - std::memcpy(longitudinal_autosar_bytes, &longitudinal_autosar, sizeof(longitudinal_autosar)); - std::memcpy(lateral_autosar_bytes, &lateral_autosar, sizeof(lateral_autosar)); - std::memcpy(vertical_autosar_bytes, &vertical_autosar, sizeof(vertical_autosar)); - std::memcpy(yaw_autosar_bytes, &yaw_autosar, sizeof(yaw_autosar)); - std::memcpy(pitch_autosar_bytes, &pitch_autosar, sizeof(pitch_autosar)); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 0] = longitudinal_autosar_bytes[3]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 1] = longitudinal_autosar_bytes[2]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 2] = longitudinal_autosar_bytes[1]; - send_vector[CONFIGURATION_LONGITUDINAL_BYTE + 3] = longitudinal_autosar_bytes[0]; - - send_vector[CONFIGURATION_LATERAL_BYTE + 0] = lateral_autosar_bytes[3]; - send_vector[CONFIGURATION_LATERAL_BYTE + 1] = lateral_autosar_bytes[2]; - send_vector[CONFIGURATION_LATERAL_BYTE + 2] = lateral_autosar_bytes[1]; - send_vector[CONFIGURATION_LATERAL_BYTE + 3] = lateral_autosar_bytes[0]; - - send_vector[CONFIGURATION_VERTICAL_BYTE + 0] = vertical_autosar_bytes[3]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 1] = vertical_autosar_bytes[2]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 2] = vertical_autosar_bytes[1]; - send_vector[CONFIGURATION_VERTICAL_BYTE + 3] = vertical_autosar_bytes[0]; - - send_vector[CONFIGURATION_YAW_BYTE + 0] = yaw_autosar_bytes[3]; - send_vector[CONFIGURATION_YAW_BYTE + 1] = yaw_autosar_bytes[2]; - send_vector[CONFIGURATION_YAW_BYTE + 2] = yaw_autosar_bytes[1]; - send_vector[CONFIGURATION_YAW_BYTE + 3] = yaw_autosar_bytes[0]; - - send_vector[CONFIGURATION_PITCH_BYTE + 0] = pitch_autosar_bytes[3]; - send_vector[CONFIGURATION_PITCH_BYTE + 1] = pitch_autosar_bytes[2]; - send_vector[CONFIGURATION_PITCH_BYTE + 2] = pitch_autosar_bytes[1]; - send_vector[CONFIGURATION_PITCH_BYTE + 3] = pitch_autosar_bytes[0]; - - send_vector[CONFIGURATION_PLUG_ORIENTATION_BYTE] = plug_orientation; - send_vector[CONFIGURATION_NEW_SENSOR_MOUNTING_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.longitudinal = longitudinal_autosar; - configuration.configuration.lateral = lateral_autosar; - configuration.configuration.vertical = vertical_autosar; - configuration.configuration.yaw = yaw_autosar; - configuration.configuration.pitch = pitch_autosar; - configuration.configuration.plug_orientation = plug_orientation; - configuration.new_sensor_mounting = 1; - - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + ConfigurationPacket configuration_packet{}; + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.longitudinal = longitudinal_autosar; + configuration_packet.configuration.lateral = lateral_autosar; + configuration_packet.configuration.vertical = vertical_autosar; + configuration_packet.configuration.yaw = yaw_autosar; + configuration_packet.configuration.pitch = pitch_autosar; + configuration_packet.configuration.plug_orientation = plug_orientation; + configuration_packet.new_sensor_mounting = 1; + + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -661,12 +405,6 @@ Status ContinentalARS548HwInterface::SetSensorMounting( Status ContinentalARS548HwInterface::SetVehicleParameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) { - constexpr int CONFIGURATION_LENGTH_BYTE = 29; - constexpr int CONFIGURATION_WIDTH_BYTE = 33; - constexpr int CONFIGURATION_HEIGHT_BYTE = 37; - constexpr int CONFIGURATION_WHEEL_BASE_BYTE = 41; - constexpr int CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE = 61; - if ( length_autosar < 0.01f || length_autosar > 100.f || width_autosar < 0.01f || width_autosar > 100.f || height_autosar < 0.01f || height_autosar > 100.f || @@ -675,59 +413,19 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - uint8_t length_autosar_bytes[4]; - uint8_t width_autosar_bytes[4]; - uint8_t height_autosar_bytes[4]; - uint8_t wheel_base_autosar_bytes[4]; - std::memcpy(length_autosar_bytes, &length_autosar, sizeof(length_autosar)); - std::memcpy(width_autosar_bytes, &width_autosar, sizeof(width_autosar)); - std::memcpy(height_autosar_bytes, &height_autosar, sizeof(height_autosar)); - std::memcpy(wheel_base_autosar_bytes, &wheel_base_autosar, sizeof(wheel_base_autosar)); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_LENGTH_BYTE + 0] = length_autosar_bytes[3]; - send_vector[CONFIGURATION_LENGTH_BYTE + 1] = length_autosar_bytes[2]; - send_vector[CONFIGURATION_LENGTH_BYTE + 2] = length_autosar_bytes[1]; - send_vector[CONFIGURATION_LENGTH_BYTE + 3] = length_autosar_bytes[0]; - - send_vector[CONFIGURATION_WIDTH_BYTE + 0] = width_autosar_bytes[3]; - send_vector[CONFIGURATION_WIDTH_BYTE + 1] = width_autosar_bytes[2]; - send_vector[CONFIGURATION_WIDTH_BYTE + 2] = width_autosar_bytes[1]; - send_vector[CONFIGURATION_WIDTH_BYTE + 3] = width_autosar_bytes[0]; - - send_vector[CONFIGURATION_HEIGHT_BYTE + 0] = height_autosar_bytes[3]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 1] = height_autosar_bytes[2]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 2] = height_autosar_bytes[1]; - send_vector[CONFIGURATION_HEIGHT_BYTE + 3] = height_autosar_bytes[0]; - - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 0] = wheel_base_autosar_bytes[3]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 1] = wheel_base_autosar_bytes[2]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 2] = wheel_base_autosar_bytes[1]; - send_vector[CONFIGURATION_WHEEL_BASE_BYTE + 3] = wheel_base_autosar_bytes[0]; - - send_vector[CONFIGURATION_NEW_VEHICLE_PARAMETERS_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.length = length_autosar; - configuration.configuration.width = width_autosar; - configuration.configuration.height = height_autosar; - configuration.configuration.wheelbase = wheel_base_autosar; - configuration.new_vehicle_parameters = 1; + ConfigurationPacket configuration_packet{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.length = length_autosar; + configuration_packet.configuration.width = width_autosar; + configuration_packet.configuration.height = height_autosar; + configuration_packet.configuration.wheelbase = wheel_base_autosar; + configuration_packet.new_vehicle_parameters = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -738,14 +436,6 @@ Status ContinentalARS548HwInterface::SetRadarParameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t power_save_standstill) { - constexpr int CONFIGURATION_MAXIMUM_DISTANCE_BYTE = 45; - constexpr int CONFIGURATION_FREQUENCY_SLOT_BYTE = 47; - constexpr int CONFIGURATION_CYCLE_TIME_BYTE = 48; - constexpr int CONFIGURATION_TIME_SLOT_BYTE = 49; - constexpr int CONFIGURATION_HCC_BYTE = 50; - constexpr int CONFIGURATION_power_save_STANDSTILL_BYTE = 51; - constexpr int CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE = 62; - if ( maximum_distance < 93 || maximum_distance > 1514 || frequency_slot > 2 || cycle_time < 50 || cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 || @@ -754,42 +444,20 @@ Status ContinentalARS548HwInterface::SetRadarParameters( return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 0] = - static_cast((maximum_distance >> 8) & 0xff); - send_vector[CONFIGURATION_MAXIMUM_DISTANCE_BYTE + 1] = - static_cast(maximum_distance & 0xff); - - send_vector[CONFIGURATION_FREQUENCY_SLOT_BYTE] = frequency_slot; - send_vector[CONFIGURATION_CYCLE_TIME_BYTE] = cycle_time; - send_vector[CONFIGURATION_TIME_SLOT_BYTE] = time_slot; - send_vector[CONFIGURATION_HCC_BYTE] = hcc; - send_vector[CONFIGURATION_power_save_STANDSTILL_BYTE] = power_save_standstill; - - send_vector[CONFIGURATION_NEW_RADAR_PARAMETERS_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; - configuration.configuration.maximum_distance = maximum_distance; - configuration.configuration.frequency_slot = frequency_slot; - configuration.configuration.cycle_time = cycle_time; - configuration.configuration.hcc = hcc; - configuration.configuration.powersave_standstill = power_save_standstill; - configuration.new_radar_parameters = 1; + ConfigurationPacket configuration_packet{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); + configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; + configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; + configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.configuration.maximum_distance = maximum_distance; + configuration_packet.configuration.frequency_slot = frequency_slot; + configuration_packet.configuration.cycle_time = cycle_time; + configuration_packet.configuration.hcc = hcc; + configuration_packet.configuration.powersave_standstill = power_save_standstill; + configuration_packet.new_radar_parameters = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -798,10 +466,6 @@ Status ContinentalARS548HwInterface::SetRadarParameters( Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) { - constexpr int CONFIGURATION_SENSOR_IP_ADDRESS0 = 52; - constexpr int CONFIGURATION_SENSOR_IP_ADDRESS1 = 56; - constexpr int CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE = 63; - std::array ip_bytes; try { @@ -812,29 +476,8 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens return Status::SENSOR_CONFIG_ERROR; } - std::vector send_vector(CONFIGURATION_PAYLOAD_LENGTH + 8); - - send_vector[METHOD_ID_BYTE + 0] = static_cast((CONFIGURATION_METHOD_ID >> 8) & 0xff); - send_vector[METHOD_ID_BYTE + 1] = static_cast(CONFIGURATION_METHOD_ID & 0xff); - send_vector[LENGTH_BYTE + 0] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 24) & 0xff); - send_vector[LENGTH_BYTE + 1] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 16) & 0xff); - send_vector[LENGTH_BYTE + 2] = static_cast((CONFIGURATION_PAYLOAD_LENGTH >> 8) & 0xff); - send_vector[LENGTH_BYTE + 3] = static_cast(CONFIGURATION_PAYLOAD_LENGTH & 0xff); - - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 0] = ip_bytes[0]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 1] = ip_bytes[1]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 2] = ip_bytes[2]; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS0 + 3] = ip_bytes[3]; - - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 0] = 169; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 1] = 254; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 2] = 116; - send_vector[CONFIGURATION_SENSOR_IP_ADDRESS1 + 3] = 113; - - send_vector[CONFIGURATION_NEW_NETWORK_CONFIGURATION_BYTE] = 1; - - Configuration configuration{}; - assert(send_vector.size() == sizeof(Configuration)); + ConfigurationPacket configuration{}; + static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); configuration.header.service_id = CONFIGURATION_SERVICE_ID; configuration.header.method_id = CONFIGURATION_METHOD_ID; configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; @@ -848,9 +491,8 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens configuration.configuration.sensor_ip_address13 = 113; configuration.new_network_configuration = 1; - std::vector send_vector2(sizeof(Configuration)); - std::memcpy(send_vector2.data(), &configuration, sizeof(Configuration)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(ConfigurationPacket)); + std::memcpy(send_vector.data(), &configuration, sizeof(ConfigurationPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -862,36 +504,22 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_PAYLOAD_SIZE = ACCELERATION_LATERAL_COG_LENGTH + 8; + const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { PrintError("Invalid lateral_acceleration value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &lateral_acceleration, sizeof(lateral_acceleration)); - - std::vector send_vector(ACCELERATION_LATERAL_COG_PAYLOAD_SIZE, 0); - send_vector[1] = ACCELERATION_LATERAL_COG_SERVICE_ID; - send_vector[2] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID >> 8); - send_vector[3] = static_cast(ACCELERATION_LATERAL_COG_METHOD_ID & 0x00ff); - send_vector[7] = ACCELERATION_LATERAL_COG_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - AccelerationLateralCoG acceleration_lateral_cog{}; - assert(send_vector.size() == sizeof(AccelerationLateralCoG)); + AccelerationLateralCoGPacket acceleration_lateral_cog{}; + static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; - std::vector send_vector2(sizeof(AccelerationLateralCoG)); - std::memcpy(send_vector2.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoG)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); + std::memcpy(send_vector.data(), &acceleration_lateral_cog, sizeof(AccelerationLateralCoGPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -903,37 +531,24 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE = ACCELERATION_LONGITUDINAL_COG_LENGTH + 8; + const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { PrintError("Invalid longitudinal_acceleration value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &longitudinal_acceleration, sizeof(longitudinal_acceleration)); - - std::vector send_vector(ACCELERATION_LONGITUDINAL_COG_PAYLOAD_SIZE, 0); - send_vector[1] = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - send_vector[2] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID >> 8); - send_vector[3] = static_cast(ACCELERATION_LONGITUDINAL_COG_METHOD_ID & 0x00ff); - send_vector[7] = ACCELERATION_LONGITUDINAL_COG_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - AccelerationLongitudinalCoG acceleration_longitudinal_cog{}; - assert(send_vector.size() == sizeof(AccelerationLongitudinalCoG)); + AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; + static_assert( + sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; - std::vector send_vector2(sizeof(AccelerationLongitudinalCoG)); + std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); std::memcpy( - send_vector2.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoG)); - assert(send_vector2 == send_vector2); + send_vector.data(), &acceleration_longitudinal_cog, sizeof(AccelerationLongitudinalCoGPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -945,30 +560,22 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_PAYLOAD_SIZE = CHARACTERISTIC_SPEED_LENGTH + 8; + const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; if (characteristic_speed < 0.f || characteristic_speed > 255.f) { PrintError("Invalid characteristic_speed value"); return Status::ERROR_1; } - std::vector send_vector(CHARACTERISTIC_SPEED_PAYLOAD_SIZE, 0); - send_vector[1] = CHARACTERISTIC_SPEED_SERVICE_ID; - send_vector[2] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID >> 8); - send_vector[3] = static_cast(CHARACTERISTIC_SPEED_METHOD_ID & 0x00ff); - send_vector[7] = CHARACTERISTIC_SPEED_LENGTH; - send_vector[10] = static_cast(characteristic_speed); - - CharasteristicSpeed characteristic_speed_packet{}; - assert(send_vector.size() == sizeof(CharasteristicSpeed)); + CharasteristicSpeedPacket characteristic_speed_packet{}; + static_assert(sizeof(CharasteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; characteristic_speed_packet.characteristic_speed = characteristic_speed; - std::vector send_vector2(sizeof(CharasteristicSpeed)); - std::memcpy(send_vector2.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeed)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(CharasteristicSpeedPacket)); + std::memcpy(send_vector.data(), &characteristic_speed_packet, sizeof(CharasteristicSpeedPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -983,24 +590,10 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_PAYLOAD_SIZE = DRIVING_DIRECTION_LENGTH + 8; - - std::vector send_vector(DRIVING_DIRECTION_PAYLOAD_SIZE, 0); - send_vector[1] = DRIVING_DIRECTION_SERVICE_ID; - send_vector[2] = static_cast(DRIVING_DIRECTION_METHOD_ID >> 8); - send_vector[3] = static_cast(DRIVING_DIRECTION_METHOD_ID & 0xff); - send_vector[7] = DRIVING_DIRECTION_LENGTH; + const int DRIVING_DIRECTION_UDP_LENGTH = 30; - if (direction == 0) { - send_vector[9] = DRIVING_DIRECTION_STANDSTILL; - } else if (direction > 0) { - send_vector[9] = DRIVING_DIRECTION_FORWARD; - } else { - send_vector[9] = DRIVING_DIRECTION_BACKWARDS; - } - - DrivingDirection driving_direction_packet{}; - assert(send_vector.size() == sizeof(DrivingDirection)); + DrivingDirectionPacket driving_direction_packet{}; + static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; @@ -1013,9 +606,8 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; } - std::vector send_vector2(sizeof(DrivingDirection)); - std::memcpy(send_vector2.data(), &driving_direction_packet, sizeof(DrivingDirection)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(DrivingDirectionPacket)); + std::memcpy(send_vector.data(), &driving_direction_packet, sizeof(DrivingDirectionPacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1027,37 +619,23 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_PAYLOAD_SIZE = STEERING_ANGLE_LENGTH + 8; + const int STEERING_ANGLE_UDP_LENGTH = 40; if (angle_rad < -M_PI_2 || angle_rad > M_PI_2) { PrintError("Invalid angle_rad value"); return Status::ERROR_1; } - uint8_t bytes[4]; - std::memcpy(bytes, &angle_rad, sizeof(angle_rad)); - - std::vector send_vector(STEERING_ANGLE_PAYLOAD_SIZE, 0); - send_vector[1] = STEERING_ANGLE_SERVICE_ID; - send_vector[2] = static_cast(STEERING_ANGLE_METHOD_ID >> 8); - send_vector[3] = static_cast(STEERING_ANGLE_METHOD_ID & 0x00ff); - send_vector[7] = STEERING_ANGLE_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - SteeringAngleFrontAxle steering_angle_front_axle_packet{}; - assert(send_vector.size() == sizeof(SteeringAngleFrontAxle)); + SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; - std::vector send_vector2(sizeof(SteeringAngleFrontAxle)); + std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); std::memcpy( - send_vector2.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxle)); - assert(send_vector2 == send_vector2); + send_vector.data(), &steering_angle_front_axle_packet, sizeof(SteeringAngleFrontAxlePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1069,31 +647,17 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity) constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_PAYLOAD_SIZE = VELOCITY_VEHICLE_LENGTH + 8; - - uint8_t bytes[4]; - std::memcpy(bytes, &velocity, sizeof(velocity)); - - std::vector send_vector(VELOCITY_VEHICLE_PAYLOAD_SIZE, 0); - send_vector[1] = VELOCITY_VEHICLE_SERVICE_ID; - send_vector[2] = static_cast(VELOCITY_VEHICLE_METHOD_ID >> 8); - send_vector[3] = static_cast(VELOCITY_VEHICLE_METHOD_ID & 0x00ff); - send_vector[7] = VELOCITY_VEHICLE_LENGTH; - send_vector[11] = bytes[3]; - send_vector[12] = bytes[2]; - send_vector[13] = bytes[1]; - send_vector[14] = bytes[0]; - - VelocityVehicle steering_angle_front_axle_packet{}; - assert(send_vector.size() == sizeof(VelocityVehicle)); + const int VELOCITY_VEHICLE_UDP_SIZE = 36; + + VelocityVehiclePacket steering_angle_front_axle_packet{}; + static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); steering_angle_front_axle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; steering_angle_front_axle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; steering_angle_front_axle_packet.header.length = VELOCITY_VEHICLE_LENGTH; steering_angle_front_axle_packet.velocity_vehicle = velocity; - std::vector send_vector2(sizeof(VelocityVehicle)); - std::memcpy(send_vector2.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehicle)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(VelocityVehiclePacket)); + std::memcpy(send_vector.data(), &steering_angle_front_axle_packet, sizeof(VelocityVehiclePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); @@ -1105,31 +669,17 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) constexpr uint16_t YAW_RATE_SERVICE_ID = 0; constexpr uint16_t YAW_RATE_METHOD_ID = 326; constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_PAYLOAD_SIZE = YAW_RATE_LENGTH + 8; - - uint8_t bytes[4]; - std::memcpy(bytes, &yaw_rate, sizeof(yaw_rate)); - - std::vector send_vector(YAW_RATE_PAYLOAD_SIZE, 0); - send_vector[1] = YAW_RATE_SERVICE_ID; - send_vector[2] = static_cast(YAW_RATE_METHOD_ID >> 8); - send_vector[3] = static_cast(YAW_RATE_METHOD_ID & 0x00ff); - send_vector[7] = YAW_RATE_LENGTH; - send_vector[14] = bytes[3]; - send_vector[15] = bytes[2]; - send_vector[16] = bytes[1]; - send_vector[17] = bytes[0]; - - YawRate yaw_rate_packet{}; - assert(send_vector.size() == sizeof(YawRate)); + const int YAW_RATE_UDP_SIZE = 40; + + YawRatePacket yaw_rate_packet{}; + static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; yaw_rate_packet.header.length = YAW_RATE_LENGTH; yaw_rate_packet.yaw_rate = yaw_rate; - std::vector send_vector2(sizeof(YawRate)); - std::memcpy(send_vector2.data(), &yaw_rate_packet, sizeof(YawRate)); - assert(send_vector2 == send_vector2); + std::vector send_vector(sizeof(YawRatePacket)); + std::memcpy(send_vector.data(), &yaw_rate_packet, sizeof(YawRatePacket)); sensor_udp_driver_->sender()->asyncSend(send_vector); diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp index 1234abd06..33aee3622 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp @@ -308,14 +308,14 @@ void ContinentalARS548HwInterfaceRosWrapper::ContinentalMonitorStatus( diagnostics.add("sensor_ip_address0", sensor_status.sensor_ip_address0); diagnostics.add("sensor_ip_address1", sensor_status.sensor_ip_address1); diagnostics.add("configuration_counter", std::to_string(sensor_status.configuration_counter)); - diagnostics.add("status_longitudinal_velocity", sensor_status.status_longitudinal_velocity); + diagnostics.add("longitudinal_velocity_status", sensor_status.longitudinal_velocity_status); diagnostics.add( - "status_longitudinal_acceleration", sensor_status.status_longitudinal_acceleration); - diagnostics.add("status_lateral_acceleration", sensor_status.status_lateral_acceleration); - diagnostics.add("status_yaw_rate", sensor_status.status_yaw_rate); - diagnostics.add("status_steering_angle", sensor_status.status_steering_angle); - diagnostics.add("status_driving_direction", sensor_status.status_driving_direction); - diagnostics.add("characteristic_speed", sensor_status.characteristic_speed); + "longitudinal_acceleration_status", sensor_status.longitudinal_acceleration_status); + diagnostics.add("lateral_acceleration_status", sensor_status.lateral_acceleration_status); + diagnostics.add("yaw_rate_status", sensor_status.yaw_rate_status); + diagnostics.add("steering_angle_status", sensor_status.steering_angle_status); + diagnostics.add("driving_direction_status", sensor_status.driving_direction_status); + diagnostics.add("characteristic_speed_status", sensor_status.characteristic_speed_status); diagnostics.add("radar_status", sensor_status.radar_status); diagnostics.add("voltage_status", sensor_status.voltage_status); diagnostics.add("temperature_status", sensor_status.temperature_status);