Skip to content

Commit 514f537

Browse files
ClemensLinnhoffClemensLinnhoff
committed
added exemplary reflections and performance logging for flatbuffers
Signed-off-by: linnhoff <linnhoff@fzd.tu-darmstadt.de>
1 parent 7f5a0a7 commit 514f537

File tree

2 files changed

+214
-6
lines changed

2 files changed

+214
-6
lines changed

‎examples/OSMPDummySensor_flat/OSMPDummySensor.cpp‎

Lines changed: 109 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -111,6 +111,7 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo)
111111

112112
bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data)
113113
{
114+
//todo: sensor view config currently not implemented
114115
/*if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) {
115116
void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX]);
116117
normal_log("OSMP","Got %08X %08X, reading from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX],buffer);
@@ -119,17 +120,17 @@ bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration&
119120
} else {
120121
return false;
121122
}*/
122-
return false; //todo
123+
return false;
123124
}
124125

125126
void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data)
126127
{
128+
//todo: sensor view config currently not implemented
127129
/*data.SerializeToString(currentConfigRequestBuffer);
128130
encode_pointer_to_integer(currentConfigRequestBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]);
129131
integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=(fmi2Integer)currentConfigRequestBuffer->length();
130132
normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX],currentConfigRequestBuffer->data());
131133
swap(currentConfigRequestBuffer,lastConfigRequestBuffer);*/
132-
//todo
133134
}
134135

135136
void COSMPDummySensor::reset_fmi_sensor_view_config_request()
@@ -182,6 +183,7 @@ void COSMPDummySensor::reset_fmi_sensor_data_out()
182183

183184
void COSMPDummySensor::refresh_fmi_sensor_view_config_request()
184185
{
186+
//todo: sensor view config currently not implemented
185187
/*osi3::SensorViewConfiguration config;
186188
if (get_fmi_sensor_view_config(config))
187189
set_fmi_sensor_view_config_request(config);
@@ -199,7 +201,6 @@ void COSMPDummySensor::refresh_fmi_sensor_view_config_request()
199201
generic->set_field_of_view_vertical(3.14);
200202
set_fmi_sensor_view_config_request(config);
201203
}*/
202-
//todo
203204
}
204205

205206
/*
@@ -285,9 +286,54 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
285286
DEBUGBREAK();
286287
flatbuffers::FlatBufferBuilder builder(1024);
287288
double time = currentCommunicationPoint+communicationStepSize;
289+
std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
288290
normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize);
289291
const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in();
292+
std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
293+
290294
if (sensor_view_in) {
295+
//// Lidar Detections
296+
std::vector<flatbuffers::Offset<osi3::LidarDetection>> lidar_detection_vector;
297+
if (sensor_view_in->lidar_sensor_view()) {
298+
int no_of_layers = 32; // the number of layers of every lidar front-end
299+
double azimuth_fov = 360.0; // Azimuth angle FoV in °
300+
int rays_per_beam_vertical = 3; // vertical super-sampling factor
301+
int rays_per_beam_horizontal = 6; // horizontal super-sampling factor
302+
double beam_step_azimuth = 0.2; // horizontal step-size per beam in degrees of VLP32 at 600 rpm (10 Hz) with VLP32's fixed firing_cycle of 55.296e^(-6) s
303+
double beam_step_elevation = 0.3; // simplified equidistant beam spacing
304+
double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB
305+
int rays_per_layer = azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8;
306+
double const_distance = 10.0;
307+
double speed_of_light = 299792458.0;
308+
size_t num_reflections = sensor_view_in->lidar_sensor_view()->Get(0)->reflection()->size();
309+
int layer_idx = -1;
310+
for (size_t reflection_idx = 0; reflection_idx < num_reflections; reflection_idx++) {
311+
if ((reflection_idx % rays_per_layer) == 0) layer_idx++;
312+
auto current_reflection = sensor_view_in->lidar_sensor_view()->Get(0)->reflection()->Get(reflection_idx);
313+
if (reflection_idx % 18 == 0) { //18 times super-sampling
314+
//todo: generate lidar detection
315+
double distance = current_reflection->time_of_flight() * speed_of_light / 2;
316+
double azimuth_deg = double(reflection_idx % rays_per_layer) * beam_step_azimuth;
317+
double elevation_deg = layer_idx * beam_step_elevation - 5; //start at -5° for simplification
318+
auto detection_position = osi3::CreateSpherical3d(builder, distance, azimuth_deg*M_PI/180, elevation_deg*M_PI/180);
319+
osi3::LidarDetectionBuilder lidar_detection_builder(builder);
320+
lidar_detection_builder.add_position(detection_position);
321+
lidar_detection_vector.push_back(lidar_detection_builder.Finish());
322+
}
323+
}
324+
}
325+
auto lidar_detection_vector_flatvector = builder.CreateVector(lidar_detection_vector);
326+
auto lidar_sensor_builder = osi3::LidarDetectionDataBuilder(builder);
327+
lidar_sensor_builder.add_detection(lidar_detection_vector_flatvector);
328+
auto lidar_sensor = lidar_sensor_builder.Finish();
329+
std::vector<flatbuffers::Offset<osi3::LidarDetectionData>> lidar_sensor_vector;
330+
lidar_sensor_vector.push_back(lidar_sensor);
331+
auto lidar_sensor_vector_flatvector = builder.CreateVector(lidar_sensor_vector);
332+
auto feature_data_builder = osi3::FeatureDataBuilder(builder);
333+
feature_data_builder.add_lidar_sensor(lidar_sensor_vector_flatvector);
334+
auto feature_data = feature_data_builder.Finish();
335+
336+
//// Moving Objects
291337
double ego_x=0, ego_y=0, ego_z=0;
292338
const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id();
293339
normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id->value());
@@ -383,8 +429,13 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
383429
sensor_data_builder.add_timestamp(timestamp);
384430
//sensor_data_builder.add_version(interface_version);
385431
sensor_data_builder.add_moving_object(detected_moving_object_flatvector);
432+
if (sensor_view_in->lidar_sensor_view()) {
433+
sensor_data_builder.add_feature_data(feature_data);
434+
}
386435
auto sensor_data = sensor_data_builder.Finish();
387436

437+
std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
438+
388439
builder.Finish(sensor_data);
389440
auto uint8_buffer = builder.GetBufferPointer();
390441
auto size = builder.GetSize();
@@ -397,6 +448,61 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
397448
set_fmi_valid(true);
398449
set_fmi_count(moving_obj_counter);
399450

451+
std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
452+
453+
//// Performance logging
454+
std::ifstream f(fileName.c_str());
455+
bool fileExists = f.is_open();
456+
f.close();
457+
458+
std::ofstream logFile;
459+
if(!fileExists) {
460+
auto now = std::chrono::system_clock::now();
461+
auto in_time_t = std::chrono::system_clock::to_time_t(now);
462+
std::stringstream time_string;
463+
time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json");
464+
fileName += time_string.str();
465+
logFile.open (fileName, std::ios_base::app);
466+
logFile << "{" << std::endl;
467+
logFile << "\t\"Header\": {" << std::endl;
468+
logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl;
469+
logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl;
470+
logFile << "\t\t\"EventTypes\": [\"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl;
471+
logFile << "\t\t\"FormatVersion\": {" << std::endl;
472+
logFile << "\t\t\t\"Major\": 1," << std::endl;
473+
logFile << "\t\t\t\"Minor\": 0," << std::endl;
474+
logFile << "\t\t\t\"Patch\": 0," << std::endl;
475+
logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl;
476+
logFile << "\t\t}" << std::endl;
477+
logFile << "\t}," << std::endl;
478+
logFile << "\t\"Data\": [" << std::endl;
479+
logFile << "\t\t{" << std::endl;
480+
logFile << "\t\t\t\"Instance\": {" << std::endl;
481+
logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Flatbuf\"" << std::endl;
482+
/*logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Flatbuf\"" << "," << std::endl;
483+
logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl;
484+
logFile << "\t\t\t\t\t\"version_major\": " << sensor_view_in->version()->version_major() << "," << std::endl;
485+
logFile << "\t\t\t\t\t\"version_minor\": " << sensor_view_in->version()->version_minor() << "," << std::endl;
486+
logFile << "\t\t\t\t\t\"version_patch\": " << sensor_view_in->version()->version_patch() << std::endl;*
487+
logFile << "\t\t\t\t}" << std::endl;*/
488+
logFile << "\t\t\t}," << std::endl;
489+
logFile << "\t\t\t\"OsiEvents\": [" << std::endl;
490+
} else {
491+
logFile.open (fileName, std::ios_base::app);
492+
}
493+
494+
if(fileExists) {
495+
logFile << "," << std::endl;
496+
}
497+
size_t sensorDataSize = builder.GetSize();
498+
double osiSimTime = (double)sensor_view_in->global_ground_truth()->timestamp()->seconds() + (double)sensor_view_in->global_ground_truth()->timestamp()->nanos() * 0.000000001;
499+
500+
logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl;
501+
logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl;
502+
logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl;
503+
logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]";
504+
logFile.close();
505+
400506
} else {
401507
/* We have no valid input, so no valid output */
402508
normal_log("OSI","No valid input, therefore providing no valid output.");

0 commit comments

Comments
 (0)