Skip to content

Commit

Permalink
feat: implemented changes from another branch (for the ars548). diano…
Browse files Browse the repository at this point in the history 
…stics are now parsed in the decoder, fixed radar configuration, and implemented a temporary multi radar hw interface

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
  • Loading branch information
@knzo25
knzo25 committed Feb 16, 2024
1 parent cfd2b7c commit 098f400
Show file tree
Hide file tree
Showing 38 changed files with 2,668 additions and 1,009 deletions.
306 changes: 270 additions & 36 deletions nebula_common/include/nebula_common/continental/continental_ars548.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,17 +13,27 @@
// limitations under the License.

#pragma once
/**
* Continental ARS548
*/

#include <nebula_common/nebula_common.hpp>
#include <nebula_common/nebula_status.hpp>

#include "boost/endian/buffers.hpp"
#include <boost/algorithm/string/join.hpp>
#include <boost/format.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <bitset>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <ctime>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

namespace nebula
{
Expand All @@ -32,6 +42,230 @@ namespace drivers
namespace continental_ars548
{

/// @brief struct for ARS548 sensor configuration
struct ContinentalARS548SensorConfiguration : EthernetSensorConfigurationBase
{
std::string multicast_ip{};
std::string new_sensor_ip{};
std::string base_frame{};
uint16_t configuration_host_port{};
uint16_t configuration_sensor_port{};
bool use_sensor_time{};
uint16_t new_plug_orientation{};
float new_vehicle_length{};
float new_vehicle_width{};
float new_vehicle_height{};
float new_vehicle_wheelbase{};
uint16_t new_radar_maximum_distance{};
uint16_t new_radar_frequency_slot{};
uint16_t new_radar_cycle_time{};
uint16_t new_radar_time_slot{};
uint16_t new_radar_country_code{};
uint16_t new_radar_powersave_standstill{};
};

/// @brief struct for Multiple ARS548 sensor configuration
struct MultiContinentalARS548SensorConfiguration : ContinentalARS548SensorConfiguration
{
std::vector<std::string> sensor_ips{};
std::vector<std::string> frame_ids{};
};

/// @brief Convert ContinentalARS548SensorConfiguration to string (Overloading the <<
/// operator)
/// @param os
/// @param arg
/// @return stream
inline std::ostream & operator<<(
std::ostream & os, ContinentalARS548SensorConfiguration const & arg)
{
os << (EthernetSensorConfigurationBase)(arg) << ", MulticastIP: " << arg.multicast_ip
<< ", NewSensorIP: " << arg.new_sensor_ip << ", BaseFrame: " << arg.base_frame
<< ", ConfigurationHostPort: " << arg.configuration_host_port
<< ", ConfigurationSensorPort: " << arg.configuration_sensor_port
<< ", UseSensorTime: " << arg.use_sensor_time
<< ", NewPlugOrientation: " << arg.new_plug_orientation
<< ", NewVehicleLength: " << arg.new_vehicle_length
<< ", NewVehicleWidth: " << arg.new_vehicle_width
<< ", NewVehicleHeight: " << arg.new_vehicle_height
<< ", NewVehicleWheelbase: " << arg.new_vehicle_wheelbase
<< ", NewRadarMaximumDistance: " << arg.new_radar_maximum_distance
<< ", NewRadarFrequencySlot: " << arg.new_radar_frequency_slot
<< ", NewRadarCycleTime: " << arg.new_radar_cycle_time
<< ", NewRadarTimeSlot: " << arg.new_radar_time_slot
<< ", NewRadarCountryCode: " << arg.new_radar_country_code
<< ", RadarPowersaveStandstill: " << arg.new_radar_powersave_standstill;
return os;
}

/// @brief Convert MultiContinentalARS548SensorConfiguration to string (Overloading the <<
/// operator)
/// @param os
/// @param arg
/// @return stream
inline std::ostream & operator<<(
std::ostream & os, MultiContinentalARS548SensorConfiguration const & arg)
{
std::stringstream sensor_ips_ss;
sensor_ips_ss << "[";
for (const auto sensor_ip : arg.sensor_ips) {
sensor_ips_ss << sensor_ip << ", ";
}
sensor_ips_ss << "]";

std::stringstream frame_ids_ss;
frame_ids_ss << "[";
for (const auto frame_id : arg.frame_ids) {
frame_ids_ss << frame_id << ", ";
}
frame_ids_ss << "]";

os << (ContinentalARS548SensorConfiguration)(arg) << ", MulticastIP: " << arg.multicast_ip
<< ", SensorIPs: " << sensor_ips_ss.str() << ", FrameIds: " << frame_ids_ss.str();
return os;
}

/// @brief semantic struct of ARS548 Status
struct ContinentalARS548Status
{
// Filled with raw sensor data
uint32_t timestamp_nanoseconds{};
uint32_t timestamp_seconds{};
std::string timestamp_sync_status{};
uint8_t sw_version_major{};
uint8_t sw_version_minor{};
uint8_t sw_version_patch{};
float longitudinal{};
float lateral{};
float vertical{};
float yaw{};
float pitch{};
std::string plug_orientation{};
float length{};
float width{};
float height{};
float wheel_base{};
uint16_t max_distance{};
std::string frequency_slot{};
uint8_t cycle_time{};
uint8_t time_slot{};
std::string hcc{};
std::string power_save_standstill{};
std::string sensor_ip_address0{};
std::string sensor_ip_address1{};
uint8_t configuration_counter{};
std::string longitudinal_velocity_status{};
std::string longitudinal_acceleration_status{};
std::string lateral_acceleration_status{};
std::string yaw_rate_status{};
std::string steering_angle_status{};
std::string driving_direction_status{};
std::string characteristic_speed_status{};
std::string radar_status{};
std::string voltage_status{};
std::string temperature_status{};
std::string blockage_status{};

// Processed data
uint64_t detection_first_stamp{};
uint64_t detection_last_stamp{};
uint64_t detection_total_count{};
uint64_t detection_dropped_dt_count{};
uint64_t detection_empty_count{};

uint64_t object_first_stamp{};
uint64_t object_last_stamp{};
uint64_t object_total_count{};
uint64_t object_dropped_dt_count{};
uint64_t object_empty_count{};

uint64_t status_total_count{};
uint64_t radar_invalid_count{};

ContinentalARS548Status() {}

/// @brief Stream ContinentalRadarStatus method
/// @param os
/// @param arg
/// @return stream
friend std::ostream & operator<<(std::ostream & os, ContinentalARS548Status const & arg)
{
os << "timestamp_nanoseconds: " << arg.timestamp_nanoseconds;
os << ", ";
os << "timestamp_seconds: " << arg.timestamp_seconds;
os << ", ";
os << "timestamp_sync_status: " << arg.timestamp_sync_status;
os << ", ";
os << "sw_version_major: " << arg.sw_version_major;
os << ", ";
os << "sw_version_minor: " << arg.sw_version_minor;
os << ", ";
os << "sw_version_patch: " << arg.sw_version_patch;
os << ", ";
os << "longitudinal: " << arg.longitudinal;
os << ", ";
os << "lateral: " << arg.lateral;
os << ", ";
os << "vertical: " << arg.vertical;
os << ", ";
os << "yaw: " << arg.yaw;
os << ", ";
os << "pitch: " << arg.pitch;
os << ", ";
os << "plug_orientation: " << arg.plug_orientation;
os << ", ";
os << "length: " << arg.length;
os << ", ";
os << "width: " << arg.width;
os << ", ";
os << "height: " << arg.height;
os << ", ";
os << "wheel_base: " << arg.wheel_base;
os << ", ";
os << "max_distance: " << arg.max_distance;
os << ", ";
os << "frequency_slot: " << arg.frequency_slot;
os << ", ";
os << "cycle_time: " << arg.cycle_time;
os << ", ";
os << "time_slot: " << arg.time_slot;
os << ", ";
os << "hcc: " << arg.hcc;
os << ", ";
os << "power_save_standstill: " << arg.power_save_standstill;
os << ", ";
os << "sensor_ip_address0: " << arg.sensor_ip_address0;
os << ", ";
os << "sensor_ip_address1: " << arg.sensor_ip_address1;
os << ", ";
os << "configuration_counter: " << arg.configuration_counter;
os << ", ";
os << "status_longitudinal_velocity: " << arg.longitudinal_velocity_status;
os << ", ";
os << "status_longitudinal_acceleration: " << arg.longitudinal_acceleration_status;
os << ", ";
os << "status_lateral_acceleration: " << arg.lateral_acceleration_status;
os << ", ";
os << "status_yaw_rate: " << arg.yaw_rate_status;
os << ", ";
os << "status_steering_angle: " << arg.steering_angle_status;
os << ", ";
os << "status_driving_direction: " << arg.driving_direction_status;
os << ", ";
os << "characteristic_speed: " << arg.characteristic_speed_status;
os << ", ";
os << "radar_status: " << arg.radar_status;
os << ", ";
os << "temperature_status: " << arg.temperature_status;
os << ", ";
os << "voltage_status: " << arg.voltage_status;
os << ", ";
os << "blockage_status: " << arg.blockage_status;

return os;
}
};

using boost::endian::big_float32_buf_t;
using boost::endian::big_uint16_buf_t;
using boost::endian::big_uint32_buf_t;
Expand Down Expand Up @@ -66,9 +300,9 @@ constexpr int MAX_OBJECTS = 50;

struct HeaderPacket
{
big_uint16_buf_t service_id;
big_uint16_buf_t method_id;
big_uint32_buf_t length;
big_uint16_buf_t service_id{};
big_uint16_buf_t method_id{};
big_uint32_buf_t length{};
};

struct HeaderSomeIPPacket
Expand Down Expand Up @@ -231,30 +465,30 @@ struct ObjectListPacket

struct StatusConfigurationPacket
{
big_float32_buf_t longitudinal;
big_float32_buf_t lateral;
big_float32_buf_t vertical;
big_float32_buf_t yaw;
big_float32_buf_t pitch;
uint8_t plug_orientation;
big_float32_buf_t length;
big_float32_buf_t width;
big_float32_buf_t height;
big_float32_buf_t wheelbase;
big_uint16_buf_t maximum_distance;
uint8_t frequency_slot;
uint8_t cycle_time;
uint8_t time_slot;
uint8_t hcc;
uint8_t powersave_standstill;
uint8_t sensor_ip_address00;
uint8_t sensor_ip_address01;
uint8_t sensor_ip_address02;
uint8_t sensor_ip_address03;
uint8_t sensor_ip_address10;
uint8_t sensor_ip_address11;
uint8_t sensor_ip_address12;
uint8_t sensor_ip_address13;
big_float32_buf_t longitudinal{};
big_float32_buf_t lateral{};
big_float32_buf_t vertical{};
big_float32_buf_t yaw{};
big_float32_buf_t pitch{};
uint8_t plug_orientation{};
big_float32_buf_t length{};
big_float32_buf_t width{};
big_float32_buf_t height{};
big_float32_buf_t wheelbase{};
big_uint16_buf_t maximum_distance{};
uint8_t frequency_slot{};
uint8_t cycle_time{};
uint8_t time_slot{};
uint8_t hcc{};
uint8_t powersave_standstill{};
uint8_t sensor_ip_address00{};
uint8_t sensor_ip_address01{};
uint8_t sensor_ip_address02{};
uint8_t sensor_ip_address03{};
uint8_t sensor_ip_address10{};
uint8_t sensor_ip_address11{};
uint8_t sensor_ip_address12{};
uint8_t sensor_ip_address13{};
};

struct SensorStatusPacket
Expand All @@ -281,12 +515,12 @@ struct SensorStatusPacket

struct ConfigurationPacket
{
HeaderPacket header;
StatusConfigurationPacket configuration;
uint8_t new_sensor_mounting;
uint8_t new_vehicle_parameters;
uint8_t new_radar_parameters;
uint8_t new_network_configuration;
HeaderPacket header{};
StatusConfigurationPacket configuration{};
uint8_t new_sensor_mounting{};
uint8_t new_vehicle_parameters{};
uint8_t new_radar_parameters{};
uint8_t new_network_configuration{};
};

struct AccelerationLateralCoGPacket
Expand Down
Loading

0 comments on commit 098f400

Please sign in to comment.