Skip to content

Commit 490d984

Browse files
ClemensLinnhoffClemensLinnhoff
committed
added reflections and performance logging to protobuf examples
Signed-off-by: linnhoff <linnhoff@fzd.tu-darmstadt.de>
1 parent 514f537 commit 490d984

File tree

2 files changed

+234
-16
lines changed

2 files changed

+234
-16
lines changed

‎examples/OSMPDummySensor/OSMPDummySensor.cpp‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,13 +48,23 @@
4848
#include <algorithm>
4949
#include <cstdint>
5050
#include <cmath>
51+
#include <chrono>
52+
#include <ctime>
53+
#include <iomanip>
54+
#include <sstream> //included for windows compatibility
5155

5256
using namespace std;
5357

5458
#ifdef PRIVATE_LOG_PATH
5559
ofstream COSMPDummySensor::private_log_file;
5660
#endif
5761

62+
#ifdef _WIN32
63+
std::string fileName = "C:/tmp/OSMPDummySensor_Protobuf_timing";
64+
#else
65+
std::string fileName = "/tmp/OSMPDummySensor_Protobuf_timing";
66+
#endif
67+
5868
/*
5969
* ProtocolBuffer Accessors
6070
*/
@@ -133,6 +143,7 @@ bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data)
133143
if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) {
134144
void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX]);
135145
normal_log("OSMP","Got %08X %08X, reading from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX],buffer);
146+
std::printf("Got %08X %08X, reading from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX],buffer);
136147
data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]);
137148
return true;
138149
} else {
@@ -263,7 +274,41 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
263274
osi3::SensorData currentOut;
264275
double time = currentCommunicationPoint+communicationStepSize;
265276
normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize);
277+
std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
266278
if (get_fmi_sensor_view_in(currentIn)) {
279+
std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
280+
//// Lidar Detections
281+
if (currentIn.lidar_sensor_view_size() > 0) {
282+
auto lidar_sensor = currentOut.mutable_feature_data()->add_lidar_sensor();
283+
int no_of_layers = 32; // the number of layers of every lidar front-end
284+
double azimuth_fov = 360.0; // Azimuth angle FoV in °
285+
int rays_per_beam_vertical = 3; // vertical super-sampling factor
286+
int rays_per_beam_horizontal = 6; // horizontal super-sampling factor
287+
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
288+
double beam_step_elevation = 0.3; // simplified equidistant beam spacing
289+
double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB
290+
int rays_per_layer = azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8;
291+
double const_distance = 10.0;
292+
double speed_of_light = 299792458.0;
293+
size_t num_reflections = currentIn.lidar_sensor_view(0).reflection_size();
294+
int layer_idx = -1;
295+
for (int reflection_idx = 0; reflection_idx < num_reflections; reflection_idx++) {
296+
if ((reflection_idx % rays_per_layer) == 0) layer_idx++;
297+
auto current_reflection = currentIn.lidar_sensor_view(0).reflection(reflection_idx);
298+
if (reflection_idx % 18 == 0) { //18 times super-sampling
299+
//todo: generate lidar detection
300+
double distance = current_reflection.time_of_flight() * speed_of_light / 2;
301+
double azimuth_deg = double(reflection_idx % rays_per_layer) * beam_step_azimuth;
302+
double elevation_deg = layer_idx * beam_step_elevation - 5; //start at -5° for simplification
303+
auto current_detection = lidar_sensor->add_detection();
304+
current_detection->mutable_position()->set_distance(distance);
305+
current_detection->mutable_position()->set_azimuth(azimuth_deg*M_PI/180);
306+
current_detection->mutable_position()->set_elevation(elevation_deg*M_PI/180);
307+
}
308+
}
309+
}
310+
311+
//// Moving Objects
267312
double ego_x=0, ego_y=0, ego_z=0;
268313
osi3::Identifier ego_id = currentIn.global_ground_truth().host_vehicle_id();
269314
normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id.value());
@@ -333,10 +378,66 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
333378
}
334379
});
335380
normal_log("OSI","Mapped %d vehicles to output", i);
381+
336382
/* Serialize */
383+
std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
337384
set_fmi_sensor_data_out(currentOut);
338385
set_fmi_valid(true);
339386
set_fmi_count(currentOut.moving_object_size());
387+
std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch());
388+
389+
//Performance logging
390+
std::ifstream f(fileName.c_str());
391+
bool fileExists = f.is_open();
392+
f.close();
393+
394+
std::ofstream logFile;
395+
if(!fileExists) {
396+
auto now = std::chrono::system_clock::now();
397+
auto in_time_t = std::chrono::system_clock::to_time_t(now);
398+
std::stringstream time_string;
399+
time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json");
400+
fileName += time_string.str();
401+
logFile.open (fileName, std::ios_base::app);
402+
logFile << "{" << std::endl;
403+
logFile << "\t\"Header\": {" << std::endl;
404+
logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl;
405+
logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl;
406+
logFile << "\t\t\"EventTypes\": [\"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl;
407+
logFile << "\t\t\"FormatVersion\": {" << std::endl;
408+
logFile << "\t\t\t\"Major\": 1," << std::endl;
409+
logFile << "\t\t\t\"Minor\": 0," << std::endl;
410+
logFile << "\t\t\t\"Patch\": 0," << std::endl;
411+
logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl;
412+
logFile << "\t\t}" << std::endl;
413+
logFile << "\t}," << std::endl;
414+
logFile << "\t\"Data\": [" << std::endl;
415+
logFile << "\t\t{" << std::endl;
416+
logFile << "\t\t\t\"Instance\": {" << std::endl;
417+
logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Protobuf\"" << std::endl;
418+
/*logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Protobuf\"" << "," << std::endl;
419+
logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl;
420+
logFile << "\t\t\t\t\t\"version_major\": " << sensor_view_in->version()->version_major() << "," << std::endl;
421+
logFile << "\t\t\t\t\t\"version_minor\": " << sensor_view_in->version()->version_minor() << "," << std::endl;
422+
logFile << "\t\t\t\t\t\"version_patch\": " << sensor_view_in->version()->version_patch() << std::endl;*
423+
logFile << "\t\t\t\t}" << std::endl;*/
424+
logFile << "\t\t\t}," << std::endl;
425+
logFile << "\t\t\t\"OsiEvents\": [" << std::endl;
426+
} else {
427+
logFile.open (fileName, std::ios_base::app);
428+
}
429+
430+
if(fileExists) {
431+
logFile << "," << std::endl;
432+
}
433+
size_t sensorDataSize = currentOut.ByteSize();
434+
double osiSimTime = (double)currentOut.timestamp().seconds() + (float)currentOut.timestamp().nanos() * 0.000000001;
435+
logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; //todo: sensor view size
436+
logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl;
437+
logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl;
438+
logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]";
439+
logFile.close();
440+
340441
} else {
341442
/* We have no valid input, so no valid output */
342443
normal_log("OSI","No valid input, therefore providing no valid output.");
@@ -661,6 +762,13 @@ extern "C" {
661762
FMI2_Export fmi2Status fmi2Terminate(fmi2Component c)
662763
{
663764
COSMPDummySensor* myc = (COSMPDummySensor*)c;
765+
std::ofstream logFile;
766+
logFile.open(fileName, std::ios_base::app);
767+
logFile << std::endl << "\t\t\t]" << std::endl;
768+
logFile << "\t\t}" << std::endl;
769+
logFile << "\t]" << std::endl;
770+
logFile << "}" << std::endl;
771+
logFile.close();
664772
return myc->Terminate();
665773
}
666774

0 commit comments

Comments
 (0)