From c4cd0d24dc3eb4ba60d9ded40bfe7ae4b90f095b Mon Sep 17 00:00:00 2001 From: linnhoff Date: Fri, 28 Jan 2022 14:13:55 +0100 Subject: [PATCH 01/13] Initial DummySource and DummySensor conversion to Flatbuffers Signed-off-by: linnhoff --- examples/CMakeLists.txt | 5 +- examples/OSMPDummySensor/CMakeLists.txt | 8 +- examples/OSMPDummySensor/OSMPDummySensor.cpp | 265 ++++++++++++------- examples/OSMPDummySensor/OSMPDummySensor.h | 19 +- examples/OSMPDummySource/CMakeLists.txt | 6 +- examples/OSMPDummySource/OSMPDummySource.cpp | 188 ++++++++----- examples/OSMPDummySource/OSMPDummySource.h | 14 +- 7 files changed, 318 insertions(+), 187 deletions(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 0e9e2d9..e9b9c30 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_minimum_required(VERSION 3.5) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/Modules/") +set(BUILD_FLATBUFFER ON CACHE BOOLEAN "Build flatbuffer versions of libraries") # Set a default build type if none was specified set(default_build_type "Release") @@ -28,4 +29,6 @@ set(OSIVERSION "${OSI_VERSION_MAJOR}.${OSI_VERSION_MINOR}.${OSI_VERSION_PATCH}") include_directories( includes ) add_subdirectory( OSMPDummySensor ) add_subdirectory( OSMPDummySource ) -add_subdirectory( OSMPCNetworkProxy ) +#add_subdirectory( OSMPCNetworkProxy ) +#add_subdirectory( object-based-generic-perception-object-model ) +#add_subdirectory( reflection-based-lidar-object-model ) diff --git a/examples/OSMPDummySensor/CMakeLists.txt b/examples/OSMPDummySensor/CMakeLists.txt index 625725d..fbf8a42 100644 --- a/examples/OSMPDummySensor/CMakeLists.txt +++ b/examples/OSMPDummySensor/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySensor) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) @@ -23,16 +23,16 @@ string(MD5 FMUGUID modelDescription.in.xml) configure_file(modelDescription.in.xml modelDescription.xml @ONLY) configure_file(OSMPDummySensorConfig.in.h OSMPDummySensorConfig.h) -find_package(Protobuf 2.6.1 REQUIRED) +find_package(Protobuf 3.0.0 REQUIRED) add_library(OSMPDummySensor SHARED OSMPDummySensor.cpp) set_target_properties(OSMPDummySensor PROPERTIES PREFIX "") target_compile_definitions(OSMPDummySensor PRIVATE "FMU_SHARED_OBJECT") if(LINK_WITH_SHARED_OSI) - target_link_libraries(OSMPDummySensor open_simulation_interface) + target_link_libraries(OSMPDummySensor open_simulation_interface_fbs) else() target_link_libraries(OSMPDummySensor open_simulation_interface_pic) endif() -include_directories(${CMAKE_CURRENT_BINARY_DIR}) +include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") if(WIN32) if(CMAKE_SIZEOF_VOID_P EQUAL 8) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index fe5f62b..105f920 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -48,6 +48,7 @@ #include #include #include +#include using namespace std; @@ -55,11 +56,18 @@ using namespace std; ofstream COSMPDummySensor::private_log_file; #endif +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySensor_flatbuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySensor_flatbuf_timing"; +#endif + + /* - * ProtocolBuffer Accessors + * Buffer Accessors */ -void* decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) +void * decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) { #if PTRDIFF_MAX == INT64_MAX union addrconv { @@ -72,6 +80,7 @@ void* decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) myaddr.base.lo=lo; myaddr.base.hi=hi; return reinterpret_cast(myaddr.address); + #elif PTRDIFF_MAX == INT32_MAX return reinterpret_cast(lo); #else @@ -102,23 +111,25 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data) { - if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { + /*if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX]); 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); data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX]); return true; } else { return false; - } + }*/ + return false; //todo } void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data) { - data.SerializeToString(currentConfigRequestBuffer); + /*data.SerializeToString(currentConfigRequestBuffer); encode_pointer_to_integer(currentConfigRequestBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]); integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=(fmi2Integer)currentConfigRequestBuffer->length(); 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()); - swap(currentConfigRequestBuffer,lastConfigRequestBuffer); + swap(currentConfigRequestBuffer,lastConfigRequestBuffer);*/ + //todo } void COSMPDummySensor::reset_fmi_sensor_view_config_request() @@ -128,25 +139,38 @@ void COSMPDummySensor::reset_fmi_sensor_view_config_request() integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]=0; } -bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data) +const osi3::SensorView* COSMPDummySensor::get_fmi_sensor_view_in() { if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX]); 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); - data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); - return true; + 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); + //data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); + + //Experimental: Read from file + /*std::ifstream infile; + infile.open("/tmp/data.bin", std::ios::binary | std::ios::in); + infile.seekg(0, std::ios::end); + int length = infile.tellg(); + infile.seekg(0, std::ios::beg); + char* data = new char[length]; + infile.read(data, length); + infile.close();*/ + + auto sensor_view_in = flatbuffers::GetRoot(buffer); + return sensor_view_in; } else { - return false; + return nullptr; } } -void COSMPDummySensor::set_fmi_sensor_data_out(const osi3::SensorData& data) +void COSMPDummySensor::set_fmi_sensor_data_out() { - data.SerializeToString(currentOutputBuffer); - encode_pointer_to_integer(currentOutputBuffer->data(),integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]); - integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer->length(); - normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer->data()); - swap(currentOutputBuffer,lastOutputBuffer); + encode_pointer_to_integer(currentOutputBuffer.data(),integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer.length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); + std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); + //swap(currentOutputBuffer,lastOutputBuffer); } void COSMPDummySensor::reset_fmi_sensor_data_out() @@ -158,12 +182,12 @@ void COSMPDummySensor::reset_fmi_sensor_data_out() void COSMPDummySensor::refresh_fmi_sensor_view_config_request() { - osi3::SensorViewConfiguration config; + /*osi3::SensorViewConfiguration config; if (get_fmi_sensor_view_config(config)) set_fmi_sensor_view_config_request(config); else { config.Clear(); - config.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); + config.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options()->)GetExtension(osi3::current_interface_version)); config.set_field_of_view_horizontal(3.14); config.set_field_of_view_vertical(3.14); config.set_range(fmi_nominal_range()*1.1); @@ -174,7 +198,8 @@ void COSMPDummySensor::refresh_fmi_sensor_view_config_request() generic->set_field_of_view_horizontal(3.14); generic->set_field_of_view_vertical(3.14); set_fmi_sensor_view_config_request(config); - } + }*/ + //todo } /* @@ -223,15 +248,15 @@ fmi2Status COSMPDummySensor::doExitInitializationMode() { DEBUGBREAK(); - osi3::SensorViewConfiguration config; + /*osi3::SensorViewConfiguration config; if (!get_fmi_sensor_view_config(config)) normal_log("OSI","Received no valid SensorViewConfiguration from Simulation Environment, assuming everything checks out."); else { - normal_log("OSI","Received SensorViewConfiguration for Sensor Id %llu",config.sensor_id().value()); + normal_log("OSI","Received SensorViewConfiguration for Sensor Id %llu",config.sensor_id()->)value()); normal_log("OSI","SVC Ground Truth FoV Horizontal %f, FoV Vertical %f, Range %f",config.field_of_view_horizontal(),config.field_of_view_vertical(),config.range()); - normal_log("OSI","SVC Mounting Position: (%f, %f, %f)",config.mounting_position().position().x(),config.mounting_position().position().y(),config.mounting_position().position().z()); - normal_log("OSI","SVC Mounting Orientation: (%f, %f, %f)",config.mounting_position().orientation().roll(),config.mounting_position().orientation().pitch(),config.mounting_position().orientation().yaw()); - } + normal_log("OSI","SVC Mounting Position: (%f, %f, %f)",config.mounting_position()->)position()->)x(),config.mounting_position()->)position()->)y(),config.mounting_position()->)position()->)z()); + normal_log("OSI","SVC Mounting Orientation: (%f, %f, %f)",config.mounting_position()->)orientation()->)roll(),config.mounting_position()->)orientation()->)pitch(),config.mounting_position()->)orientation()->)yaw()); + }*/ return fmi2OK; } @@ -258,85 +283,120 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - - osi3::SensorView currentIn; - osi3::SensorData currentOut; + flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); - if (get_fmi_sensor_view_in(currentIn)) { + const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in(); + if (sensor_view_in) { double ego_x=0, ego_y=0, ego_z=0; - osi3::Identifier ego_id = currentIn.global_ground_truth().host_vehicle_id(); - normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id.value()); - for_each(currentIn.global_ground_truth().moving_object().begin(),currentIn.global_ground_truth().moving_object().end(), - [this, ego_id, &ego_x, &ego_y, &ego_z](const osi3::MovingObject& obj) { - normal_log("OSI","MovingObject with ID %llu is EgoVehicle: %d",obj.id().value(), obj.id().value() == ego_id.value()); - if (obj.id().value() == ego_id.value()) { - normal_log("OSI","Found EgoVehicle with ID: %llu",obj.id().value()); - ego_x = obj.base().position().x(); - ego_y = obj.base().position().y(); - ego_z = obj.base().position().z(); - } - }); + const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id(); + normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id->value()); + size_t num_moving_obj = sensor_view_in->global_ground_truth()->moving_object()->size(); + for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { + auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); + normal_log("OSI", "MovingObject with ID %llu is EgoVehicle: %d", current_obj->id()->value(), current_obj->id()->value() == ego_id->value()); + if (current_obj->id()->value() == ego_id->value()) { + normal_log("OSI", "Found EgoVehicle with ID: %llu", current_obj->id()->value()); + ego_x = current_obj->base()->position()->x(); + ego_y = current_obj->base()->position()->y(); + ego_z = current_obj->base()->position()->z(); + } + } normal_log("OSI","Current Ego Position: %f,%f,%f", ego_x, ego_y, ego_z); - /* Clear Output */ - currentOut.Clear(); - currentOut.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); - /* Adjust Timestamps and Ids */ - currentOut.mutable_timestamp()->set_seconds((long long int)floor(time)); - currentOut.mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); - /* Copy of SensorView */ - currentOut.add_sensor_view()->CopyFrom(currentIn); - - int i=0; + std::vector> detected_moving_object_vector; + int moving_obj_counter=0; double actual_range = fmi_nominal_range()*1.1; - for_each(currentIn.global_ground_truth().moving_object().begin(),currentIn.global_ground_truth().moving_object().end(), - [this,&i,¤tIn,¤tOut,ego_id,ego_x,ego_y,ego_z,actual_range](const osi3::MovingObject& veh) { - if (veh.id().value() != ego_id.value()) { - // NOTE: We currently do not take sensor mounting position into account, - // i.e. sensor-relative coordinates are relative to center of bounding box - // of ego vehicle currently. - double trans_x = veh.base().position().x()-ego_x; - double trans_y = veh.base().position().y()-ego_y; - double trans_z = veh.base().position().z()-ego_z; - double rel_x,rel_y,rel_z; - rotatePoint(trans_x,trans_y,trans_z,veh.base().orientation().yaw(),veh.base().orientation().pitch(),veh.base().orientation().roll(),rel_x,rel_y,rel_z); - double distance = sqrt(rel_x*rel_x + rel_y*rel_y + rel_z*rel_z); - if ((distance <= actual_range) && (rel_x/distance > 0.866025)) { - osi3::DetectedMovingObject *obj = currentOut.mutable_moving_object()->Add(); - obj->mutable_header()->add_ground_truth_id()->CopyFrom(veh.id()); - obj->mutable_header()->mutable_tracking_id()->set_value(i); - obj->mutable_header()->set_existence_probability(cos((2.0*distance-actual_range)/actual_range)); - obj->mutable_header()->set_measurement_state(osi3::DetectedItemHeader_MeasurementState_MEASUREMENT_STATE_MEASURED); - obj->mutable_header()->add_sensor_id()->CopyFrom(currentIn.sensor_id()); - obj->mutable_base()->mutable_position()->set_x(rel_x); - obj->mutable_base()->mutable_position()->set_y(rel_y); - obj->mutable_base()->mutable_position()->set_z(rel_z); - obj->mutable_base()->mutable_dimension()->set_length(veh.base().dimension().length()); - obj->mutable_base()->mutable_dimension()->set_width(veh.base().dimension().width()); - obj->mutable_base()->mutable_dimension()->set_height(veh.base().dimension().height()); - - osi3::DetectedMovingObject::CandidateMovingObject* candidate = obj->add_candidate(); - candidate->set_type(veh.type()); - candidate->mutable_vehicle_classification()->CopyFrom(veh.vehicle_classification()); - candidate->set_probability(1); - - normal_log("OSI","Output Vehicle %d[%llu] Probability %f Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),obj->header().existence_probability(),rel_x,rel_y,rel_z,obj->base().position().x(),obj->base().position().y(),obj->base().position().z()); - i++; - } else { - normal_log("OSI","Ignoring Vehicle %d[%llu] Outside Sensor Scope Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),veh.base().position().x()-ego_x,veh.base().position().y()-ego_y,veh.base().position().z()-ego_z,veh.base().position().x(),veh.base().position().y(),veh.base().position().z()); - } - } - else - { - normal_log("OSI","Ignoring EGO Vehicle %d[%llu] Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),veh.base().position().x()-ego_x,veh.base().position().y()-ego_y,veh.base().position().z()-ego_z,veh.base().position().x(),veh.base().position().y(),veh.base().position().z()); + + for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { + auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); + if (current_obj->id()->value() != ego_id->value()) { + // NOTE: We currently do not take sensor mounting position into account, + // i.e. sensor-relative coordinates are relative to center of bounding box + // of ego vehicle currently. + double trans_x = current_obj->base()->position()->x() - ego_x; + double trans_y = current_obj->base()->position()->y() - ego_y; + double trans_z = current_obj->base()->position()->z() - ego_z; + double rel_x,rel_y,rel_z; + rotatePoint(trans_x, trans_y, trans_z, current_obj->base()->orientation()->yaw(), current_obj->base()->orientation()->pitch(), current_obj->base()->orientation()->roll(), rel_x, rel_y, rel_z); + double distance = sqrt(rel_x*rel_x + rel_y*rel_y + rel_z*rel_z); + if ((distance <= actual_range) && (rel_x/distance > 0.866025)) { + std::vector> obj_gt_id; + obj_gt_id.push_back(osi3::CreateIdentifier(builder, current_obj->id()->value())); + auto obj_gt_id_flatvector = builder.CreateVector(obj_gt_id); + auto obj_tracking_id = osi3::CreateIdentifier(builder, moving_obj_counter); + std::vector> sensor_id; + sensor_id.push_back(osi3::CreateIdentifier(builder, sensor_view_in->sensor_id()->value())); + auto sensor_id_flatvector = builder.CreateVector(sensor_id); + osi3::DetectedItemHeaderBuilder detected_item_header_builder(builder); + detected_item_header_builder.add_ground_truth_id(obj_gt_id_flatvector); + detected_item_header_builder.add_tracking_id(obj_tracking_id); + detected_item_header_builder.add_existence_probability(cos((2.0*distance-actual_range)/actual_range)); + detected_item_header_builder.add_measurement_state(osi3::DetectedItemHeader_::MeasurementState::MEASUREMENT_STATE_MEASURED); + detected_item_header_builder.add_sensor_id(sensor_id_flatvector); + auto obj_header = detected_item_header_builder.Finish(); + + + auto obj_position = osi3::CreateVector3d(builder, rel_x, rel_y, rel_z); + auto obj_dimension = osi3::CreateDimension3d(builder, current_obj->base()->dimension()->length(), current_obj->base()->dimension()->width(), current_obj->base()->dimension()->height()); + osi3::BaseMovingBuilder base_moving_builder(builder); + base_moving_builder.add_position(obj_position); + base_moving_builder.add_dimension(obj_dimension); + auto base_moving = base_moving_builder.Finish(); + + std::vector> candidate_vector; + osi3::DetectedMovingObject_::CandidateMovingObjectBuilder candidate_builder(builder); + //candidate_builder.add_type(veh->type()); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused + candidate_builder.add_probability(1); + candidate_vector.push_back(candidate_builder.Finish()); + auto candidate_flatvector = builder.CreateVector(candidate_vector); + + osi3::DetectedMovingObjectBuilder detected_moving_object_builder(builder); + detected_moving_object_builder.add_header(obj_header); + detected_moving_object_builder.add_base(base_moving); + detected_moving_object_builder.add_candidate(candidate_flatvector); + auto detected_moving_object = detected_moving_object_builder.Finish(); + detected_moving_object_vector.push_back(detected_moving_object); + + auto obj = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - detected_moving_object.o); + + normal_log("OSI","Output Vehicle %d[%llu] Probability %f Relative Position: %f,%f,%f (%f,%f,%f)",obj_idx,current_obj->id()->value(),obj->header()->existence_probability(),rel_x,rel_y,rel_z,obj->base()->position()->x(),obj->base()->position()->y(),obj->base()->position()->z()); + moving_obj_counter++; + } else { + normal_log("OSI", "Ignoring Vehicle %d[%llu] Outside Sensor Scope Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); } - }); - normal_log("OSI","Mapped %d vehicles to output", i); - /* Serialize */ - set_fmi_sensor_data_out(currentOut); + } + else + { + normal_log("OSI", "Ignoring EGO Vehicle %d[%llu] Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); + } + } + auto detected_moving_object_flatvector = builder.CreateVector(detected_moving_object_vector); + + //auto interface_version = osi3::CreateInterfaceVersion(builder,sensor_view_in->version()->version_major(), sensor_view_in->version()->version_minor(), sensor_view_in->version()->version_patch()); //todo: not implemented in source + auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); + + /* Copy SensorView */ + //currentOut.add_sensor_view()->CopyFrom(sensor_view_in); //todo: copying a serialized buffer into another buffer is not that easy in Flatbuffers + + osi3::SensorDataBuilder sensor_data_builder(builder); + sensor_data_builder.add_timestamp(timestamp); + //sensor_data_builder.add_version(interface_version); + sensor_data_builder.add_moving_object(detected_moving_object_flatvector); + auto sensor_data = sensor_data_builder.Finish(); + + builder.Finish(sensor_data); + auto uint8_buffer = builder.GetBufferPointer(); + auto size = builder.GetSize(); + std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); + currentOutputBuffer = tmp_buffer; + + normal_log("OSI", "Mapped %d vehicles to output", moving_obj_counter); + + set_fmi_sensor_data_out(); set_fmi_valid(true); - set_fmi_count(currentOut.moving_object_size()); + set_fmi_count(moving_obj_counter); + } else { /* We have no valid input, so no valid output */ normal_log("OSI","No valid input, therefore providing no valid output."); @@ -373,10 +433,10 @@ COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuTy loggingOn(!!theloggingOn), simulation_started(false) { - currentOutputBuffer=new string(); + /*currentOutputBuffer=new string(); lastOutputBuffer=new string(); currentConfigRequestBuffer=new string(); - lastConfigRequestBuffer=new string(); + lastConfigRequestBuffer=new string();*/ loggingCategories.clear(); loggingCategories.insert("FMI"); loggingCategories.insert("OSMP"); @@ -385,10 +445,10 @@ COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuTy COSMPDummySensor::~COSMPDummySensor() { - delete currentOutputBuffer; + /*delete currentOutputBuffer; delete lastOutputBuffer; delete currentConfigRequestBuffer; - delete lastConfigRequestBuffer; + delete lastConfigRequestBuffer;*/ } fmi2Status COSMPDummySensor::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) @@ -661,6 +721,13 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySensor* myc = (COSMPDummySensor*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); return myc->Terminate(); } diff --git a/examples/OSMPDummySensor/OSMPDummySensor.h b/examples/OSMPDummySensor/OSMPDummySensor.h index 889f6b0..78b730c 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.h +++ b/examples/OSMPDummySensor/OSMPDummySensor.h @@ -82,8 +82,10 @@ using namespace std; #undef min #undef max -#include "osi_sensorview.pb.h" -#include "osi_sensordata.pb.h" +#include "osi_sensorview_generated.h" +#include "osi_sensordata_generated.h" +#include "flatbuffers/reflection.h" +#include "flatbuffers/util.h" /* FMU Class */ class COSMPDummySensor { @@ -204,10 +206,10 @@ class COSMPDummySensor { fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; bool simulation_started; - string* currentOutputBuffer; - string* lastOutputBuffer; - string* currentConfigRequestBuffer; - string* lastConfigRequestBuffer; + string currentOutputBuffer; + string lastOutputBuffer; + string currentConfigRequestBuffer; + string lastConfigRequestBuffer; /* Simple Accessors */ fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } @@ -222,8 +224,9 @@ class COSMPDummySensor { bool get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data); void set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data); void reset_fmi_sensor_view_config_request(); - bool get_fmi_sensor_view_in(osi3::SensorView& data); - void set_fmi_sensor_data_out(const osi3::SensorData& data); + //bool get_fmi_sensor_view_in(osi3::SensorView& data); + const osi3::SensorView* get_fmi_sensor_view_in(); + void set_fmi_sensor_data_out(); void reset_fmi_sensor_data_out(); /* Refreshing of Calculated Parameters */ diff --git a/examples/OSMPDummySource/CMakeLists.txt b/examples/OSMPDummySource/CMakeLists.txt index e7c2f48..1d53d13 100644 --- a/examples/OSMPDummySource/CMakeLists.txt +++ b/examples/OSMPDummySource/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySource) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) @@ -28,11 +28,11 @@ add_library(OSMPDummySource SHARED OSMPDummySource.cpp) set_target_properties(OSMPDummySource PROPERTIES PREFIX "") target_compile_definitions(OSMPDummySource PRIVATE "FMU_SHARED_OBJECT") if(LINK_WITH_SHARED_OSI) - target_link_libraries(OSMPDummySource open_simulation_interface) + target_link_libraries(OSMPDummySource open_simulation_interface_fbs) else() target_link_libraries(OSMPDummySource open_simulation_interface_pic) endif() -include_directories(${CMAKE_CURRENT_BINARY_DIR}) +include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") if(WIN32) if(CMAKE_SIZEOF_VOID_P EQUAL 8) diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index b0e17e0..b1c16d4 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -44,10 +44,14 @@ #endif #include -#include +//#include #include #include #include +#include +#include +#include +#include //included for windows compatibility using namespace std; @@ -55,6 +59,12 @@ using namespace std; ofstream COSMPDummySource::private_log_file; #endif +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySource_flatbuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySource_flatbuf_timing"; +#endif + /* * ProtocolBuffer Accessors */ @@ -89,9 +99,11 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) } base; unsigned long long address; } myaddr; + myaddr.address=reinterpret_cast(ptr); hi=myaddr.base.hi; lo=myaddr.base.lo; + #elif PTRDIFF_MAX == INT32_MAX hi=0; lo=reinterpret_cast(ptr); @@ -100,13 +112,16 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) #endif } -void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) +//void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) +void COSMPDummySource::set_fmi_sensor_view_out() { - data.SerializeToString(currentBuffer); - encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); - integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer->length(); - normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer->data()); - swap(currentBuffer,lastBuffer); + //data.SerializeToString(currentBuffer); + //encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); + encode_pointer_to_integer(currentBuffer.data(), integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX], integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer.length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); + std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); + //swap(currentBuffer,lastBuffer); } void COSMPDummySource::reset_fmi_sensor_view_out() @@ -186,8 +201,7 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - - osi3::SensorView currentOut; + flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating SensorView at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); @@ -196,63 +210,93 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; static double source_x_offsets[10] = { 0.0, 40.0, 100.0, 100.0, 0.0, 150.0, 5.0, 45.0, 85.0, 125.0 }; static double source_x_speeds[10] = { 29.0, 30.0, 31.0, 25.0, 26.0, 28.0, 20.0, 22.0, 22.5, 23.0 }; - static osi3::MovingObject_VehicleClassification_Type source_veh_types[10] = { - osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN, - osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE, - osi3::MovingObject_VehicleClassification_Type_TYPE_BUS }; - - currentOut.Clear(); - currentOut.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); - currentOut.mutable_sensor_id()->set_value(10000); - currentOut.mutable_host_vehicle_id()->set_value(14); - osi3::GroundTruth *currentGT = currentOut.mutable_global_ground_truth(); - currentOut.mutable_timestamp()->set_seconds((long long int)floor(time)); - currentOut.mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); - currentGT->mutable_timestamp()->set_seconds((long long int)floor(time)); - currentGT->mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); - currentGT->mutable_host_vehicle_id()->set_value(14); - + static osi3::MovingObject_::VehicleClassification_::Type source_veh_types[10] = { + osi3::MovingObject_::VehicleClassification_::Type::TYPE_MEDIUM_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_SMALL_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_COMPACT_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_DELIVERY_VAN, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_LUXURY_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_MEDIUM_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_COMPACT_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_SMALL_CAR, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_MOTORBIKE, + osi3::MovingObject_::VehicleClassification_::Type::TYPE_BUS }; + + std::vector> moving_object_vector; // Vehicles for (unsigned int i=0;i<10;i++) { - osi3::MovingObject *veh = currentGT->add_moving_object(); - veh->mutable_id()->set_value(10+i); - veh->set_type(osi3::MovingObject_Type_TYPE_VEHICLE); - auto vehclass = veh->mutable_vehicle_classification(); - vehclass->set_type(source_veh_types[i]); - auto vehlights = vehclass->mutable_light_state(); - vehlights->set_indicator_state(osi3::MovingObject_VehicleClassification_LightState_IndicatorState_INDICATOR_STATE_OFF); - vehlights->set_brake_light_state(osi3::MovingObject_VehicleClassification_LightState_BrakeLightState_BRAKE_LIGHT_STATE_OFF); - veh->mutable_base()->mutable_dimension()->set_height(1.5); - veh->mutable_base()->mutable_dimension()->set_width(2.0); - veh->mutable_base()->mutable_dimension()->set_length(5.0); - veh->mutable_base()->mutable_position()->set_x(source_x_offsets[i]+time*source_x_speeds[i]); - veh->mutable_base()->mutable_position()->set_y(source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25); - veh->mutable_base()->mutable_position()->set_z(0.0); - veh->mutable_base()->mutable_velocity()->set_x(source_x_speeds[i]); - veh->mutable_base()->mutable_velocity()->set_y(cos(time/source_x_speeds[i])*0.25/source_x_speeds[i]); - veh->mutable_base()->mutable_velocity()->set_z(0.0); - veh->mutable_base()->mutable_acceleration()->set_x(0.0); - veh->mutable_base()->mutable_acceleration()->set_y(-sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i])); - veh->mutable_base()->mutable_acceleration()->set_z(0.0); - veh->mutable_base()->mutable_orientation()->set_pitch(0.0); - veh->mutable_base()->mutable_orientation()->set_roll(0.0); - veh->mutable_base()->mutable_orientation()->set_yaw(0.0); - veh->mutable_base()->mutable_orientation_rate()->set_pitch(0.0); - veh->mutable_base()->mutable_orientation_rate()->set_roll(0.0); - veh->mutable_base()->mutable_orientation_rate()->set_yaw(0.0); - normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,veh->id().value(),veh->base().position().x(),veh->base().position().y(),veh->base().position().z(),veh->base().velocity().x(),veh->base().velocity().y(),veh->base().velocity().z()); - } - - set_fmi_sensor_view_out(currentOut); + osi3::IdentifierBuilder id_builder(builder); + id_builder.add_value(10+i); + auto moving_obj_id = id_builder.Finish(); + + osi3::MovingObject_::VehicleClassificationBuilder vehicle_classification_builder(builder); + //vehicle_classification_builder.add_type(source_veh_types[i]); //todo: vehicle classifications are wrong in headers due to namespace conflict -> confused with moving object type + auto vehicle_classification = vehicle_classification_builder.Finish(); + + auto bbcenter_to_rear = osi3::CreateVector3d(builder, 1.0, 0.0, -0.3); + osi3::MovingObject_::VehicleAttributesBuilder vehicle_attributes_builder(builder); + vehicle_attributes_builder.add_bbcenter_to_rear(bbcenter_to_rear); + auto vehicle_attributes = vehicle_attributes_builder.Finish(); + + auto dimension = osi3::CreateDimension3d(builder, 5.0, 2.0, 1.5); + auto position = osi3::CreateVector3d(builder, source_x_offsets[i]+time*source_x_speeds[i], source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25, 0.0); + auto velocity = osi3::CreateVector3d(builder, source_x_speeds[i], cos(time/source_x_speeds[i])*0.25/source_x_speeds[i], 0.0); + auto acceleration = osi3::CreateVector3d(builder, 0.0, -sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i]), 0.0); + auto orientation = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); + auto orientation_rate = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); + osi3::BaseMovingBuilder base_moving_builder(builder); + base_moving_builder.add_dimension(dimension); + base_moving_builder.add_position(position); + base_moving_builder.add_velocity(velocity); + base_moving_builder.add_acceleration(acceleration); + base_moving_builder.add_orientation(orientation); + base_moving_builder.add_orientation_rate(orientation_rate); + auto base_moving = base_moving_builder.Finish(); + + osi3::MovingObjectBuilder moving_object_builder(builder); + moving_object_builder.add_id(moving_obj_id); + //moving_object_builder.add_type(osi3::MovingObject_::Type::TYPE_VEHICLE); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused + moving_object_builder.add_vehicle_classification(vehicle_classification); + moving_object_builder.add_vehicle_attributes(vehicle_attributes); + moving_object_builder.add_base(base_moving); + auto current_moving_object = moving_object_builder.Finish(); + moving_object_vector.push_back(current_moving_object); + + auto moving_object = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - current_moving_object.o); + normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,moving_object->id()->value(),moving_object->base()->position()->x(),moving_object->base()->position()->y(),moving_object->base()->position()->z(),moving_object->base()->velocity()->x(),moving_object->base()->velocity()->y(),moving_object->base()->velocity()->z()); + } + auto moving_object_flatvector = builder.CreateVector(moving_object_vector); + + auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); + osi3::IdentifierBuilder host_vehicle_id_builder(builder); + host_vehicle_id_builder.add_value(14); + auto host_vehicle_id = host_vehicle_id_builder.Finish(); + osi3::GroundTruthBuilder ground_truth_builder(builder); + ground_truth_builder.add_timestamp(timestamp); + ground_truth_builder.add_host_vehicle_id(host_vehicle_id); + ground_truth_builder.add_moving_object(moving_object_flatvector); + auto ground_truth = ground_truth_builder.Finish(); + + auto sensor_id = osi3::CreateIdentifier(builder, 10000); + + osi3::SensorViewBuilder sensor_view_builder(builder); + //sensor_view_builder.add_version(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); //todo: the used Protobuf FileOptions do not exist in Flatbuffers + sensor_view_builder.add_sensor_id(sensor_id); + sensor_view_builder.add_host_vehicle_id(host_vehicle_id); + sensor_view_builder.add_timestamp(timestamp); + sensor_view_builder.add_global_ground_truth(ground_truth); + auto sensor_view = sensor_view_builder.Finish(); + + builder.Finish(sensor_view); + auto uint8_buffer = builder.GetBufferPointer(); + auto size = builder.GetSize(); + std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); + currentBuffer = tmp_buffer; + + set_fmi_sensor_view_out(); set_fmi_valid(true); - set_fmi_count(currentGT->moving_object_size()); + set_fmi_count((int)moving_object_vector.size()); + return fmi2OK; } @@ -280,8 +324,10 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy visible(!!thevisible), loggingOn(!!theloggingOn) { - currentBuffer = new string(); - lastBuffer = new string(); + //currentBuffer = new string(); + //currentBuffer = new uint8_t(); + //lastBuffer = new string(); + //lastBuffer = new uint8_t(); loggingCategories.clear(); loggingCategories.insert("FMI"); loggingCategories.insert("OSMP"); @@ -290,8 +336,8 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy COSMPDummySource::~COSMPDummySource() { - delete currentBuffer; - delete lastBuffer; + //delete currentBuffer; + //delete lastBuffer; } fmi2Status COSMPDummySource::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) @@ -557,6 +603,14 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySource* myc = (COSMPDummySource*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); + return myc->Terminate(); } diff --git a/examples/OSMPDummySource/OSMPDummySource.h b/examples/OSMPDummySource/OSMPDummySource.h index 230a6e3..4f987b9 100644 --- a/examples/OSMPDummySource/OSMPDummySource.h +++ b/examples/OSMPDummySource/OSMPDummySource.h @@ -72,7 +72,7 @@ using namespace std; #undef min #undef max -#include "osi_sensorview.pb.h" +#include "osi_sensorview_generated.h" /* FMU Class */ class COSMPDummySource { @@ -192,16 +192,20 @@ class COSMPDummySource { fmi2Integer integer_vars[FMI_INTEGER_VARS]; fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; - string* currentBuffer; - string* lastBuffer; + //string* currentBuffer; + string currentBuffer; + //string* lastBuffer; + string lastBuffer; - /* Simple Accessors */ + /* Simple Accessors */ fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } void set_fmi_valid(fmi2Boolean value) { boolean_vars[FMI_BOOLEAN_VALID_IDX]=value; } fmi2Integer fmi_count() { return integer_vars[FMI_INTEGER_COUNT_IDX]; } void set_fmi_count(fmi2Integer value) { integer_vars[FMI_INTEGER_COUNT_IDX]=value; } /* Protocol Buffer Accessors */ - void set_fmi_sensor_view_out(const osi3::SensorView& data); + //void set_fmi_sensor_view_out(const osi3::SensorView& data); + void set_fmi_sensor_view_out(); void reset_fmi_sensor_view_out(); + }; From 19a64ed1acfd696f76956034e65ae912eb8e210c Mon Sep 17 00:00:00 2001 From: linnhoff Date: Fri, 4 Feb 2022 17:50:43 +0100 Subject: [PATCH 02/13] moved flatbuffers examples to separate folders Signed-off-by: linnhoff --- examples/CMakeLists.txt | 13 +- examples/OSMPDummySensor/CMakeLists.txt | 8 +- examples/OSMPDummySensor/OSMPDummySensor.cpp | 265 ++---- examples/OSMPDummySensor/OSMPDummySensor.h | 19 +- examples/OSMPDummySensor_flat/CMakeLists.txt | 67 ++ .../OSMPDummySensor_flat/OSMPDummySensor.cpp | 888 ++++++++++++++++++ .../OSMPDummySensor_flat/OSMPDummySensor.h | 234 +++++ .../OSMPDummySensorConfig.in.h | 15 + .../modelDescription.in.xml | 126 +++ examples/OSMPDummySource/CMakeLists.txt | 6 +- examples/OSMPDummySource/OSMPDummySource.cpp | 188 ++-- examples/OSMPDummySource/OSMPDummySource.h | 14 +- examples/OSMPDummySource_flat/CMakeLists.txt | 67 ++ .../OSMPDummySource_flat/OSMPDummySource.cpp | 771 +++++++++++++++ .../OSMPDummySource_flat/OSMPDummySource.h | 211 +++++ .../OSMPDummySourceConfig.in.h | 15 + .../modelDescription.in.xml | 64 ++ 17 files changed, 2652 insertions(+), 319 deletions(-) create mode 100644 examples/OSMPDummySensor_flat/CMakeLists.txt create mode 100644 examples/OSMPDummySensor_flat/OSMPDummySensor.cpp create mode 100644 examples/OSMPDummySensor_flat/OSMPDummySensor.h create mode 100644 examples/OSMPDummySensor_flat/OSMPDummySensorConfig.in.h create mode 100644 examples/OSMPDummySensor_flat/modelDescription.in.xml create mode 100644 examples/OSMPDummySource_flat/CMakeLists.txt create mode 100644 examples/OSMPDummySource_flat/OSMPDummySource.cpp create mode 100644 examples/OSMPDummySource_flat/OSMPDummySource.h create mode 100644 examples/OSMPDummySource_flat/OSMPDummySourceConfig.in.h create mode 100644 examples/OSMPDummySource_flat/modelDescription.in.xml diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index e9b9c30..5a85c56 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -27,8 +27,11 @@ get_directory_property(OSI_VERSION_PATCH DIRECTORY open-simulation-interface DEF set(OSIVERSION "${OSI_VERSION_MAJOR}.${OSI_VERSION_MINOR}.${OSI_VERSION_PATCH}") include_directories( includes ) -add_subdirectory( OSMPDummySensor ) -add_subdirectory( OSMPDummySource ) -#add_subdirectory( OSMPCNetworkProxy ) -#add_subdirectory( object-based-generic-perception-object-model ) -#add_subdirectory( reflection-based-lidar-object-model ) +if(BUILD_FLATBUFFER) + add_subdirectory( OSMPDummySensor_flat ) + add_subdirectory( OSMPDummySource_flat ) +else() + add_subdirectory( OSMPDummySensor ) + add_subdirectory( OSMPDummySource ) + add_subdirectory( OSMPCNetworkProxy ) +endif() diff --git a/examples/OSMPDummySensor/CMakeLists.txt b/examples/OSMPDummySensor/CMakeLists.txt index fbf8a42..625725d 100644 --- a/examples/OSMPDummySensor/CMakeLists.txt +++ b/examples/OSMPDummySensor/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySensor) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) @@ -23,16 +23,16 @@ string(MD5 FMUGUID modelDescription.in.xml) configure_file(modelDescription.in.xml modelDescription.xml @ONLY) configure_file(OSMPDummySensorConfig.in.h OSMPDummySensorConfig.h) -find_package(Protobuf 3.0.0 REQUIRED) +find_package(Protobuf 2.6.1 REQUIRED) add_library(OSMPDummySensor SHARED OSMPDummySensor.cpp) set_target_properties(OSMPDummySensor PROPERTIES PREFIX "") target_compile_definitions(OSMPDummySensor PRIVATE "FMU_SHARED_OBJECT") if(LINK_WITH_SHARED_OSI) - target_link_libraries(OSMPDummySensor open_simulation_interface_fbs) + target_link_libraries(OSMPDummySensor open_simulation_interface) else() target_link_libraries(OSMPDummySensor open_simulation_interface_pic) endif() -include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") +include_directories(${CMAKE_CURRENT_BINARY_DIR}) if(WIN32) if(CMAKE_SIZEOF_VOID_P EQUAL 8) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index 105f920..fe5f62b 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -48,7 +48,6 @@ #include #include #include -#include using namespace std; @@ -56,18 +55,11 @@ using namespace std; ofstream COSMPDummySensor::private_log_file; #endif -#ifdef _WIN32 -std::string fileName = "C:/tmp/OSMPDummySensor_flatbuf_timing"; -#else -std::string fileName = "/tmp/OSMPDummySensor_flatbuf_timing"; -#endif - - /* - * Buffer Accessors + * ProtocolBuffer Accessors */ -void * decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) +void* decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) { #if PTRDIFF_MAX == INT64_MAX union addrconv { @@ -80,7 +72,6 @@ void * decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) myaddr.base.lo=lo; myaddr.base.hi=hi; return reinterpret_cast(myaddr.address); - #elif PTRDIFF_MAX == INT32_MAX return reinterpret_cast(lo); #else @@ -111,25 +102,23 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data) { - /*if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { + if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX]); 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); data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX]); return true; } else { return false; - }*/ - return false; //todo + } } void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data) { - /*data.SerializeToString(currentConfigRequestBuffer); + data.SerializeToString(currentConfigRequestBuffer); encode_pointer_to_integer(currentConfigRequestBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]); integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=(fmi2Integer)currentConfigRequestBuffer->length(); 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()); - swap(currentConfigRequestBuffer,lastConfigRequestBuffer);*/ - //todo + swap(currentConfigRequestBuffer,lastConfigRequestBuffer); } void COSMPDummySensor::reset_fmi_sensor_view_config_request() @@ -139,38 +128,25 @@ void COSMPDummySensor::reset_fmi_sensor_view_config_request() integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]=0; } -const osi3::SensorView* COSMPDummySensor::get_fmi_sensor_view_in() +bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data) { if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX]); 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); - 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); - //data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); - - //Experimental: Read from file - /*std::ifstream infile; - infile.open("/tmp/data.bin", std::ios::binary | std::ios::in); - infile.seekg(0, std::ios::end); - int length = infile.tellg(); - infile.seekg(0, std::ios::beg); - char* data = new char[length]; - infile.read(data, length); - infile.close();*/ - - auto sensor_view_in = flatbuffers::GetRoot(buffer); - return sensor_view_in; + data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); + return true; } else { - return nullptr; + return false; } } -void COSMPDummySensor::set_fmi_sensor_data_out() +void COSMPDummySensor::set_fmi_sensor_data_out(const osi3::SensorData& data) { - encode_pointer_to_integer(currentOutputBuffer.data(),integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]); - integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer.length(); - normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); - std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); - //swap(currentOutputBuffer,lastOutputBuffer); + data.SerializeToString(currentOutputBuffer); + encode_pointer_to_integer(currentOutputBuffer->data(),integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer->length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer->data()); + swap(currentOutputBuffer,lastOutputBuffer); } void COSMPDummySensor::reset_fmi_sensor_data_out() @@ -182,12 +158,12 @@ void COSMPDummySensor::reset_fmi_sensor_data_out() void COSMPDummySensor::refresh_fmi_sensor_view_config_request() { - /*osi3::SensorViewConfiguration config; + osi3::SensorViewConfiguration config; if (get_fmi_sensor_view_config(config)) set_fmi_sensor_view_config_request(config); else { config.Clear(); - config.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options()->)GetExtension(osi3::current_interface_version)); + config.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); config.set_field_of_view_horizontal(3.14); config.set_field_of_view_vertical(3.14); config.set_range(fmi_nominal_range()*1.1); @@ -198,8 +174,7 @@ void COSMPDummySensor::refresh_fmi_sensor_view_config_request() generic->set_field_of_view_horizontal(3.14); generic->set_field_of_view_vertical(3.14); set_fmi_sensor_view_config_request(config); - }*/ - //todo + } } /* @@ -248,15 +223,15 @@ fmi2Status COSMPDummySensor::doExitInitializationMode() { DEBUGBREAK(); - /*osi3::SensorViewConfiguration config; + osi3::SensorViewConfiguration config; if (!get_fmi_sensor_view_config(config)) normal_log("OSI","Received no valid SensorViewConfiguration from Simulation Environment, assuming everything checks out."); else { - normal_log("OSI","Received SensorViewConfiguration for Sensor Id %llu",config.sensor_id()->)value()); + normal_log("OSI","Received SensorViewConfiguration for Sensor Id %llu",config.sensor_id().value()); normal_log("OSI","SVC Ground Truth FoV Horizontal %f, FoV Vertical %f, Range %f",config.field_of_view_horizontal(),config.field_of_view_vertical(),config.range()); - normal_log("OSI","SVC Mounting Position: (%f, %f, %f)",config.mounting_position()->)position()->)x(),config.mounting_position()->)position()->)y(),config.mounting_position()->)position()->)z()); - normal_log("OSI","SVC Mounting Orientation: (%f, %f, %f)",config.mounting_position()->)orientation()->)roll(),config.mounting_position()->)orientation()->)pitch(),config.mounting_position()->)orientation()->)yaw()); - }*/ + normal_log("OSI","SVC Mounting Position: (%f, %f, %f)",config.mounting_position().position().x(),config.mounting_position().position().y(),config.mounting_position().position().z()); + normal_log("OSI","SVC Mounting Orientation: (%f, %f, %f)",config.mounting_position().orientation().roll(),config.mounting_position().orientation().pitch(),config.mounting_position().orientation().yaw()); + } return fmi2OK; } @@ -283,120 +258,85 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - flatbuffers::FlatBufferBuilder builder(1024); + + osi3::SensorView currentIn; + osi3::SensorData currentOut; double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); - const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in(); - if (sensor_view_in) { + if (get_fmi_sensor_view_in(currentIn)) { double ego_x=0, ego_y=0, ego_z=0; - const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id(); - normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id->value()); - size_t num_moving_obj = sensor_view_in->global_ground_truth()->moving_object()->size(); - for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { - auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); - normal_log("OSI", "MovingObject with ID %llu is EgoVehicle: %d", current_obj->id()->value(), current_obj->id()->value() == ego_id->value()); - if (current_obj->id()->value() == ego_id->value()) { - normal_log("OSI", "Found EgoVehicle with ID: %llu", current_obj->id()->value()); - ego_x = current_obj->base()->position()->x(); - ego_y = current_obj->base()->position()->y(); - ego_z = current_obj->base()->position()->z(); - } - } + osi3::Identifier ego_id = currentIn.global_ground_truth().host_vehicle_id(); + normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id.value()); + for_each(currentIn.global_ground_truth().moving_object().begin(),currentIn.global_ground_truth().moving_object().end(), + [this, ego_id, &ego_x, &ego_y, &ego_z](const osi3::MovingObject& obj) { + normal_log("OSI","MovingObject with ID %llu is EgoVehicle: %d",obj.id().value(), obj.id().value() == ego_id.value()); + if (obj.id().value() == ego_id.value()) { + normal_log("OSI","Found EgoVehicle with ID: %llu",obj.id().value()); + ego_x = obj.base().position().x(); + ego_y = obj.base().position().y(); + ego_z = obj.base().position().z(); + } + }); normal_log("OSI","Current Ego Position: %f,%f,%f", ego_x, ego_y, ego_z); - std::vector> detected_moving_object_vector; - int moving_obj_counter=0; - double actual_range = fmi_nominal_range()*1.1; + /* Clear Output */ + currentOut.Clear(); + currentOut.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); + /* Adjust Timestamps and Ids */ + currentOut.mutable_timestamp()->set_seconds((long long int)floor(time)); + currentOut.mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); + /* Copy of SensorView */ + currentOut.add_sensor_view()->CopyFrom(currentIn); - for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { - auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); - if (current_obj->id()->value() != ego_id->value()) { - // NOTE: We currently do not take sensor mounting position into account, - // i.e. sensor-relative coordinates are relative to center of bounding box - // of ego vehicle currently. - double trans_x = current_obj->base()->position()->x() - ego_x; - double trans_y = current_obj->base()->position()->y() - ego_y; - double trans_z = current_obj->base()->position()->z() - ego_z; - double rel_x,rel_y,rel_z; - rotatePoint(trans_x, trans_y, trans_z, current_obj->base()->orientation()->yaw(), current_obj->base()->orientation()->pitch(), current_obj->base()->orientation()->roll(), rel_x, rel_y, rel_z); - double distance = sqrt(rel_x*rel_x + rel_y*rel_y + rel_z*rel_z); - if ((distance <= actual_range) && (rel_x/distance > 0.866025)) { - std::vector> obj_gt_id; - obj_gt_id.push_back(osi3::CreateIdentifier(builder, current_obj->id()->value())); - auto obj_gt_id_flatvector = builder.CreateVector(obj_gt_id); - auto obj_tracking_id = osi3::CreateIdentifier(builder, moving_obj_counter); - std::vector> sensor_id; - sensor_id.push_back(osi3::CreateIdentifier(builder, sensor_view_in->sensor_id()->value())); - auto sensor_id_flatvector = builder.CreateVector(sensor_id); - osi3::DetectedItemHeaderBuilder detected_item_header_builder(builder); - detected_item_header_builder.add_ground_truth_id(obj_gt_id_flatvector); - detected_item_header_builder.add_tracking_id(obj_tracking_id); - detected_item_header_builder.add_existence_probability(cos((2.0*distance-actual_range)/actual_range)); - detected_item_header_builder.add_measurement_state(osi3::DetectedItemHeader_::MeasurementState::MEASUREMENT_STATE_MEASURED); - detected_item_header_builder.add_sensor_id(sensor_id_flatvector); - auto obj_header = detected_item_header_builder.Finish(); - - - auto obj_position = osi3::CreateVector3d(builder, rel_x, rel_y, rel_z); - auto obj_dimension = osi3::CreateDimension3d(builder, current_obj->base()->dimension()->length(), current_obj->base()->dimension()->width(), current_obj->base()->dimension()->height()); - osi3::BaseMovingBuilder base_moving_builder(builder); - base_moving_builder.add_position(obj_position); - base_moving_builder.add_dimension(obj_dimension); - auto base_moving = base_moving_builder.Finish(); - - std::vector> candidate_vector; - osi3::DetectedMovingObject_::CandidateMovingObjectBuilder candidate_builder(builder); - //candidate_builder.add_type(veh->type()); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused - candidate_builder.add_probability(1); - candidate_vector.push_back(candidate_builder.Finish()); - auto candidate_flatvector = builder.CreateVector(candidate_vector); - - osi3::DetectedMovingObjectBuilder detected_moving_object_builder(builder); - detected_moving_object_builder.add_header(obj_header); - detected_moving_object_builder.add_base(base_moving); - detected_moving_object_builder.add_candidate(candidate_flatvector); - auto detected_moving_object = detected_moving_object_builder.Finish(); - detected_moving_object_vector.push_back(detected_moving_object); - - auto obj = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - detected_moving_object.o); - - normal_log("OSI","Output Vehicle %d[%llu] Probability %f Relative Position: %f,%f,%f (%f,%f,%f)",obj_idx,current_obj->id()->value(),obj->header()->existence_probability(),rel_x,rel_y,rel_z,obj->base()->position()->x(),obj->base()->position()->y(),obj->base()->position()->z()); - moving_obj_counter++; - } else { - normal_log("OSI", "Ignoring Vehicle %d[%llu] Outside Sensor Scope Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); + int i=0; + double actual_range = fmi_nominal_range()*1.1; + for_each(currentIn.global_ground_truth().moving_object().begin(),currentIn.global_ground_truth().moving_object().end(), + [this,&i,¤tIn,¤tOut,ego_id,ego_x,ego_y,ego_z,actual_range](const osi3::MovingObject& veh) { + if (veh.id().value() != ego_id.value()) { + // NOTE: We currently do not take sensor mounting position into account, + // i.e. sensor-relative coordinates are relative to center of bounding box + // of ego vehicle currently. + double trans_x = veh.base().position().x()-ego_x; + double trans_y = veh.base().position().y()-ego_y; + double trans_z = veh.base().position().z()-ego_z; + double rel_x,rel_y,rel_z; + rotatePoint(trans_x,trans_y,trans_z,veh.base().orientation().yaw(),veh.base().orientation().pitch(),veh.base().orientation().roll(),rel_x,rel_y,rel_z); + double distance = sqrt(rel_x*rel_x + rel_y*rel_y + rel_z*rel_z); + if ((distance <= actual_range) && (rel_x/distance > 0.866025)) { + osi3::DetectedMovingObject *obj = currentOut.mutable_moving_object()->Add(); + obj->mutable_header()->add_ground_truth_id()->CopyFrom(veh.id()); + obj->mutable_header()->mutable_tracking_id()->set_value(i); + obj->mutable_header()->set_existence_probability(cos((2.0*distance-actual_range)/actual_range)); + obj->mutable_header()->set_measurement_state(osi3::DetectedItemHeader_MeasurementState_MEASUREMENT_STATE_MEASURED); + obj->mutable_header()->add_sensor_id()->CopyFrom(currentIn.sensor_id()); + obj->mutable_base()->mutable_position()->set_x(rel_x); + obj->mutable_base()->mutable_position()->set_y(rel_y); + obj->mutable_base()->mutable_position()->set_z(rel_z); + obj->mutable_base()->mutable_dimension()->set_length(veh.base().dimension().length()); + obj->mutable_base()->mutable_dimension()->set_width(veh.base().dimension().width()); + obj->mutable_base()->mutable_dimension()->set_height(veh.base().dimension().height()); + + osi3::DetectedMovingObject::CandidateMovingObject* candidate = obj->add_candidate(); + candidate->set_type(veh.type()); + candidate->mutable_vehicle_classification()->CopyFrom(veh.vehicle_classification()); + candidate->set_probability(1); + + normal_log("OSI","Output Vehicle %d[%llu] Probability %f Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),obj->header().existence_probability(),rel_x,rel_y,rel_z,obj->base().position().x(),obj->base().position().y(),obj->base().position().z()); + i++; + } else { + normal_log("OSI","Ignoring Vehicle %d[%llu] Outside Sensor Scope Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),veh.base().position().x()-ego_x,veh.base().position().y()-ego_y,veh.base().position().z()-ego_z,veh.base().position().x(),veh.base().position().y(),veh.base().position().z()); + } } - } - else - { - normal_log("OSI", "Ignoring EGO Vehicle %d[%llu] Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); - } - } - auto detected_moving_object_flatvector = builder.CreateVector(detected_moving_object_vector); - - //auto interface_version = osi3::CreateInterfaceVersion(builder,sensor_view_in->version()->version_major(), sensor_view_in->version()->version_minor(), sensor_view_in->version()->version_patch()); //todo: not implemented in source - auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); - - /* Copy SensorView */ - //currentOut.add_sensor_view()->CopyFrom(sensor_view_in); //todo: copying a serialized buffer into another buffer is not that easy in Flatbuffers - - osi3::SensorDataBuilder sensor_data_builder(builder); - sensor_data_builder.add_timestamp(timestamp); - //sensor_data_builder.add_version(interface_version); - sensor_data_builder.add_moving_object(detected_moving_object_flatvector); - auto sensor_data = sensor_data_builder.Finish(); - - builder.Finish(sensor_data); - auto uint8_buffer = builder.GetBufferPointer(); - auto size = builder.GetSize(); - std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); - currentOutputBuffer = tmp_buffer; - - normal_log("OSI", "Mapped %d vehicles to output", moving_obj_counter); - - set_fmi_sensor_data_out(); + else + { + normal_log("OSI","Ignoring EGO Vehicle %d[%llu] Relative Position: %f,%f,%f (%f,%f,%f)",i,veh.id().value(),veh.base().position().x()-ego_x,veh.base().position().y()-ego_y,veh.base().position().z()-ego_z,veh.base().position().x(),veh.base().position().y(),veh.base().position().z()); + } + }); + normal_log("OSI","Mapped %d vehicles to output", i); + /* Serialize */ + set_fmi_sensor_data_out(currentOut); set_fmi_valid(true); - set_fmi_count(moving_obj_counter); - + set_fmi_count(currentOut.moving_object_size()); } else { /* We have no valid input, so no valid output */ normal_log("OSI","No valid input, therefore providing no valid output."); @@ -433,10 +373,10 @@ COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuTy loggingOn(!!theloggingOn), simulation_started(false) { - /*currentOutputBuffer=new string(); + currentOutputBuffer=new string(); lastOutputBuffer=new string(); currentConfigRequestBuffer=new string(); - lastConfigRequestBuffer=new string();*/ + lastConfigRequestBuffer=new string(); loggingCategories.clear(); loggingCategories.insert("FMI"); loggingCategories.insert("OSMP"); @@ -445,10 +385,10 @@ COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuTy COSMPDummySensor::~COSMPDummySensor() { - /*delete currentOutputBuffer; + delete currentOutputBuffer; delete lastOutputBuffer; delete currentConfigRequestBuffer; - delete lastConfigRequestBuffer;*/ + delete lastConfigRequestBuffer; } fmi2Status COSMPDummySensor::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) @@ -721,13 +661,6 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySensor* myc = (COSMPDummySensor*)c; - std::ofstream logFile; - logFile.open(fileName, std::ios_base::app); - logFile << std::endl << "\t\t\t]" << std::endl; - logFile << "\t\t}" << std::endl; - logFile << "\t]" << std::endl; - logFile << "}" << std::endl; - logFile.close(); return myc->Terminate(); } diff --git a/examples/OSMPDummySensor/OSMPDummySensor.h b/examples/OSMPDummySensor/OSMPDummySensor.h index 78b730c..889f6b0 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.h +++ b/examples/OSMPDummySensor/OSMPDummySensor.h @@ -82,10 +82,8 @@ using namespace std; #undef min #undef max -#include "osi_sensorview_generated.h" -#include "osi_sensordata_generated.h" -#include "flatbuffers/reflection.h" -#include "flatbuffers/util.h" +#include "osi_sensorview.pb.h" +#include "osi_sensordata.pb.h" /* FMU Class */ class COSMPDummySensor { @@ -206,10 +204,10 @@ class COSMPDummySensor { fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; bool simulation_started; - string currentOutputBuffer; - string lastOutputBuffer; - string currentConfigRequestBuffer; - string lastConfigRequestBuffer; + string* currentOutputBuffer; + string* lastOutputBuffer; + string* currentConfigRequestBuffer; + string* lastConfigRequestBuffer; /* Simple Accessors */ fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } @@ -224,9 +222,8 @@ class COSMPDummySensor { bool get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data); void set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data); void reset_fmi_sensor_view_config_request(); - //bool get_fmi_sensor_view_in(osi3::SensorView& data); - const osi3::SensorView* get_fmi_sensor_view_in(); - void set_fmi_sensor_data_out(); + bool get_fmi_sensor_view_in(osi3::SensorView& data); + void set_fmi_sensor_data_out(const osi3::SensorData& data); void reset_fmi_sensor_data_out(); /* Refreshing of Calculated Parameters */ diff --git a/examples/OSMPDummySensor_flat/CMakeLists.txt b/examples/OSMPDummySensor_flat/CMakeLists.txt new file mode 100644 index 0000000..fbf8a42 --- /dev/null +++ b/examples/OSMPDummySensor_flat/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(OSMPDummySensor) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") +set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") +if(WIN32) + set(PRIVATE_LOG_PATH "C:/TEMP/OSMPDummySensorLog.log" CACHE FILEPATH "Path to write private log file to") +else() + set(PRIVATE_LOG_PATH "/tmp/OSMPDummySensorLog.log" CACHE FILEPATH "Path to write private log file to") +endif() +if(PRIVATE_LOGGING) + file(TO_NATIVE_PATH ${PRIVATE_LOG_PATH} PRIVATE_LOG_PATH_NATIVE) + string(REPLACE "\\" "\\\\" PRIVATE_LOG_PATH ${PRIVATE_LOG_PATH_NATIVE}) +endif() +set(VERBOSE_FMI_LOGGING OFF CACHE BOOL "Enable detailed FMI function logging") +set(DEBUG_BREAKS OFF CACHE BOOL "Enable debugger traps for debug builds of FMU") + +string(TIMESTAMP FMUTIMESTAMP UTC) +string(MD5 FMUGUID modelDescription.in.xml) +configure_file(modelDescription.in.xml modelDescription.xml @ONLY) +configure_file(OSMPDummySensorConfig.in.h OSMPDummySensorConfig.h) + +find_package(Protobuf 3.0.0 REQUIRED) +add_library(OSMPDummySensor SHARED OSMPDummySensor.cpp) +set_target_properties(OSMPDummySensor PROPERTIES PREFIX "") +target_compile_definitions(OSMPDummySensor PRIVATE "FMU_SHARED_OBJECT") +if(LINK_WITH_SHARED_OSI) + target_link_libraries(OSMPDummySensor open_simulation_interface_fbs) +else() + target_link_libraries(OSMPDummySensor open_simulation_interface_pic) +endif() +include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") + +if(WIN32) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "win64") + else() + set(FMI_BINARIES_PLATFORM "win32") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "linux64") + else() + set(FMI_BINARIES_PLATFORM "linux32") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "darwin64") + else() + set(FMI_BINARIES_PLATFORM "darwin32") + endif() +endif() + +add_custom_command(TARGET OSMPDummySensor + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E remove_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" + COMMAND ${CMAKE_COMMAND} -E make_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources" + COMMAND ${CMAKE_COMMAND} -E make_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_BINARY_DIR}/modelDescription.xml" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/OSMPDummySensor.cpp" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/OSMPDummySensor.h" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_BINARY_DIR}/OSMPDummySensorConfig.h" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/OSMPDummySensorConfig.h" + COMMAND ${CMAKE_COMMAND} -E copy $ $<$:$<$:$>> "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}" + COMMAND ${CMAKE_COMMAND} -E chdir "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" ${CMAKE_COMMAND} -E tar "cfv" "../OSMPDummySensor.fmu" --format=zip "modelDescription.xml" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}") diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp new file mode 100644 index 0000000..105f920 --- /dev/null +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -0,0 +1,888 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include "OSMPDummySensor.h" + +/* + * Debug Breaks + * + * If you define DEBUG_BREAKS the FMU will automatically break + * into an attached Debugger on all major computation functions. + * Note that the FMU is likely to break all environments if no + * Debugger is actually attached when the breaks are triggered. + */ +#if defined(DEBUG_BREAKS) && !defined(NDEBUG) +#if defined(__has_builtin) && !defined(__ibmxl__) +#if __has_builtin(__builtin_debugtrap) +#define DEBUGBREAK() __builtin_debugtrap() +#elif __has_builtin(__debugbreak) +#define DEBUGBREAK() __debugbreak() +#endif +#endif +#if !defined(DEBUGBREAK) +#if defined(_MSC_VER) || defined(__INTEL_COMPILER) +#include +#define DEBUGBREAK() __debugbreak() +#else +#include +#if defined(SIGTRAP) +#define DEBUGBREAK() raise(SIGTRAP) +#else +#define DEBUGBREAK() raise(SIGABRT) +#endif +#endif +#endif +#else +#define DEBUGBREAK() +#endif + +#include +#include +#include +#include +#include +#include + +using namespace std; + +#ifdef PRIVATE_LOG_PATH +ofstream COSMPDummySensor::private_log_file; +#endif + +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySensor_flatbuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySensor_flatbuf_timing"; +#endif + + +/* + * Buffer Accessors + */ + +void * decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) +{ +#if PTRDIFF_MAX == INT64_MAX + union addrconv { + struct { + int lo; + int hi; + } base; + unsigned long long address; + } myaddr; + myaddr.base.lo=lo; + myaddr.base.hi=hi; + return reinterpret_cast(myaddr.address); + +#elif PTRDIFF_MAX == INT32_MAX + return reinterpret_cast(lo); +#else +#error "Cannot determine 32bit or 64bit environment!" +#endif +} + +void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) +{ +#if PTRDIFF_MAX == INT64_MAX + union addrconv { + struct { + int lo; + int hi; + } base; + unsigned long long address; + } myaddr; + myaddr.address=reinterpret_cast(ptr); + hi=myaddr.base.hi; + lo=myaddr.base.lo; +#elif PTRDIFF_MAX == INT32_MAX + hi=0; + lo=reinterpret_cast(ptr); +#else +#error "Cannot determine 32bit or 64bit environment!" +#endif +} + +bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data) +{ + /*if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { + void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX]); + 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); + data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX]); + return true; + } else { + return false; + }*/ + return false; //todo +} + +void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data) +{ + /*data.SerializeToString(currentConfigRequestBuffer); + encode_pointer_to_integer(currentConfigRequestBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=(fmi2Integer)currentConfigRequestBuffer->length(); + 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()); + swap(currentConfigRequestBuffer,lastConfigRequestBuffer);*/ + //todo +} + +void COSMPDummySensor::reset_fmi_sensor_view_config_request() +{ + integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=0; + integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX]=0; + integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]=0; +} + +const osi3::SensorView* COSMPDummySensor::get_fmi_sensor_view_in() +{ + if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) { + void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX]); + 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); + 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); + //data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); + + //Experimental: Read from file + /*std::ifstream infile; + infile.open("/tmp/data.bin", std::ios::binary | std::ios::in); + infile.seekg(0, std::ios::end); + int length = infile.tellg(); + infile.seekg(0, std::ios::beg); + char* data = new char[length]; + infile.read(data, length); + infile.close();*/ + + auto sensor_view_in = flatbuffers::GetRoot(buffer); + return sensor_view_in; + } else { + return nullptr; + } +} + +void COSMPDummySensor::set_fmi_sensor_data_out() +{ + encode_pointer_to_integer(currentOutputBuffer.data(),integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer.length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); + std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); + //swap(currentOutputBuffer,lastOutputBuffer); +} + +void COSMPDummySensor::reset_fmi_sensor_data_out() +{ + integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=0; + integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX]=0; + integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX]=0; +} + +void COSMPDummySensor::refresh_fmi_sensor_view_config_request() +{ + /*osi3::SensorViewConfiguration config; + if (get_fmi_sensor_view_config(config)) + set_fmi_sensor_view_config_request(config); + else { + config.Clear(); + config.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options()->)GetExtension(osi3::current_interface_version)); + config.set_field_of_view_horizontal(3.14); + config.set_field_of_view_vertical(3.14); + config.set_range(fmi_nominal_range()*1.1); + config.mutable_update_cycle_time()->set_seconds(0); + config.mutable_update_cycle_time()->set_nanos(20000000); + config.mutable_update_cycle_offset()->Clear(); + osi3::GenericSensorViewConfiguration* generic = config.add_generic_sensor_view_configuration(); + generic->set_field_of_view_horizontal(3.14); + generic->set_field_of_view_vertical(3.14); + set_fmi_sensor_view_config_request(config); + }*/ + //todo +} + +/* + * Actual Core Content + */ + +fmi2Status COSMPDummySensor::doInit() +{ + DEBUGBREAK(); + + /* Booleans */ + for (int i = 0; i)value()); + normal_log("OSI","SVC Ground Truth FoV Horizontal %f, FoV Vertical %f, Range %f",config.field_of_view_horizontal(),config.field_of_view_vertical(),config.range()); + normal_log("OSI","SVC Mounting Position: (%f, %f, %f)",config.mounting_position()->)position()->)x(),config.mounting_position()->)position()->)y(),config.mounting_position()->)position()->)z()); + normal_log("OSI","SVC Mounting Orientation: (%f, %f, %f)",config.mounting_position()->)orientation()->)roll(),config.mounting_position()->)orientation()->)pitch(),config.mounting_position()->)orientation()->)yaw()); + }*/ + + return fmi2OK; +} + +void rotatePoint(double x, double y, double z,double yaw,double pitch,double roll,double &rx,double &ry,double &rz) +{ + double matrix[3][3]; + double cos_yaw = cos(yaw); + double cos_pitch = cos(pitch); + double cos_roll = cos(roll); + double sin_yaw = sin(yaw); + double sin_pitch = sin(pitch); + double sin_roll = sin(roll); + + matrix[0][0] = cos_yaw*cos_pitch; matrix[0][1]=cos_yaw*sin_pitch*sin_roll - sin_yaw*cos_roll; matrix[0][2]=cos_yaw*sin_pitch*cos_roll + sin_yaw*sin_roll; + matrix[1][0] = sin_yaw*cos_pitch; matrix[1][1]=sin_yaw*sin_pitch*sin_roll + cos_yaw*cos_roll; matrix[1][2]=sin_yaw*sin_pitch*cos_roll - cos_yaw*sin_roll; + matrix[2][0] = -sin_pitch; matrix[2][1]=cos_pitch*sin_roll; matrix[2][2]=cos_pitch*cos_roll; + + rx = matrix[0][0] * x + matrix[0][1] * y + matrix[0][2] * z; + ry = matrix[1][0] * x + matrix[1][1] * y + matrix[1][2] * z; + rz = matrix[2][0] * x + matrix[2][1] * y + matrix[2][2] * z; +} + +fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) +{ + DEBUGBREAK(); + flatbuffers::FlatBufferBuilder builder(1024); + double time = currentCommunicationPoint+communicationStepSize; + normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); + const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in(); + if (sensor_view_in) { + double ego_x=0, ego_y=0, ego_z=0; + const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id(); + normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id->value()); + size_t num_moving_obj = sensor_view_in->global_ground_truth()->moving_object()->size(); + for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { + auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); + normal_log("OSI", "MovingObject with ID %llu is EgoVehicle: %d", current_obj->id()->value(), current_obj->id()->value() == ego_id->value()); + if (current_obj->id()->value() == ego_id->value()) { + normal_log("OSI", "Found EgoVehicle with ID: %llu", current_obj->id()->value()); + ego_x = current_obj->base()->position()->x(); + ego_y = current_obj->base()->position()->y(); + ego_z = current_obj->base()->position()->z(); + } + } + normal_log("OSI","Current Ego Position: %f,%f,%f", ego_x, ego_y, ego_z); + + std::vector> detected_moving_object_vector; + int moving_obj_counter=0; + double actual_range = fmi_nominal_range()*1.1; + + for (size_t obj_idx = 0; obj_idx < num_moving_obj; obj_idx++) { + auto current_obj = sensor_view_in->global_ground_truth()->moving_object()->Get(obj_idx); + if (current_obj->id()->value() != ego_id->value()) { + // NOTE: We currently do not take sensor mounting position into account, + // i.e. sensor-relative coordinates are relative to center of bounding box + // of ego vehicle currently. + double trans_x = current_obj->base()->position()->x() - ego_x; + double trans_y = current_obj->base()->position()->y() - ego_y; + double trans_z = current_obj->base()->position()->z() - ego_z; + double rel_x,rel_y,rel_z; + rotatePoint(trans_x, trans_y, trans_z, current_obj->base()->orientation()->yaw(), current_obj->base()->orientation()->pitch(), current_obj->base()->orientation()->roll(), rel_x, rel_y, rel_z); + double distance = sqrt(rel_x*rel_x + rel_y*rel_y + rel_z*rel_z); + if ((distance <= actual_range) && (rel_x/distance > 0.866025)) { + std::vector> obj_gt_id; + obj_gt_id.push_back(osi3::CreateIdentifier(builder, current_obj->id()->value())); + auto obj_gt_id_flatvector = builder.CreateVector(obj_gt_id); + auto obj_tracking_id = osi3::CreateIdentifier(builder, moving_obj_counter); + std::vector> sensor_id; + sensor_id.push_back(osi3::CreateIdentifier(builder, sensor_view_in->sensor_id()->value())); + auto sensor_id_flatvector = builder.CreateVector(sensor_id); + osi3::DetectedItemHeaderBuilder detected_item_header_builder(builder); + detected_item_header_builder.add_ground_truth_id(obj_gt_id_flatvector); + detected_item_header_builder.add_tracking_id(obj_tracking_id); + detected_item_header_builder.add_existence_probability(cos((2.0*distance-actual_range)/actual_range)); + detected_item_header_builder.add_measurement_state(osi3::DetectedItemHeader_::MeasurementState::MEASUREMENT_STATE_MEASURED); + detected_item_header_builder.add_sensor_id(sensor_id_flatvector); + auto obj_header = detected_item_header_builder.Finish(); + + + auto obj_position = osi3::CreateVector3d(builder, rel_x, rel_y, rel_z); + auto obj_dimension = osi3::CreateDimension3d(builder, current_obj->base()->dimension()->length(), current_obj->base()->dimension()->width(), current_obj->base()->dimension()->height()); + osi3::BaseMovingBuilder base_moving_builder(builder); + base_moving_builder.add_position(obj_position); + base_moving_builder.add_dimension(obj_dimension); + auto base_moving = base_moving_builder.Finish(); + + std::vector> candidate_vector; + osi3::DetectedMovingObject_::CandidateMovingObjectBuilder candidate_builder(builder); + //candidate_builder.add_type(veh->type()); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused + candidate_builder.add_probability(1); + candidate_vector.push_back(candidate_builder.Finish()); + auto candidate_flatvector = builder.CreateVector(candidate_vector); + + osi3::DetectedMovingObjectBuilder detected_moving_object_builder(builder); + detected_moving_object_builder.add_header(obj_header); + detected_moving_object_builder.add_base(base_moving); + detected_moving_object_builder.add_candidate(candidate_flatvector); + auto detected_moving_object = detected_moving_object_builder.Finish(); + detected_moving_object_vector.push_back(detected_moving_object); + + auto obj = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - detected_moving_object.o); + + normal_log("OSI","Output Vehicle %d[%llu] Probability %f Relative Position: %f,%f,%f (%f,%f,%f)",obj_idx,current_obj->id()->value(),obj->header()->existence_probability(),rel_x,rel_y,rel_z,obj->base()->position()->x(),obj->base()->position()->y(),obj->base()->position()->z()); + moving_obj_counter++; + } else { + normal_log("OSI", "Ignoring Vehicle %d[%llu] Outside Sensor Scope Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); + } + } + else + { + normal_log("OSI", "Ignoring EGO Vehicle %d[%llu] Relative Position: %f,%f,%f (%f,%f,%f)", moving_obj_counter, current_obj->id()->value(), current_obj->base()->position()->x() - ego_x, current_obj->base()->position()->y() - ego_y, current_obj->base()->position()->z() - ego_z, current_obj->base()->position()->x(), current_obj->base()->position()->y(), current_obj->base()->position()->z()); + } + } + auto detected_moving_object_flatvector = builder.CreateVector(detected_moving_object_vector); + + //auto interface_version = osi3::CreateInterfaceVersion(builder,sensor_view_in->version()->version_major(), sensor_view_in->version()->version_minor(), sensor_view_in->version()->version_patch()); //todo: not implemented in source + auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); + + /* Copy SensorView */ + //currentOut.add_sensor_view()->CopyFrom(sensor_view_in); //todo: copying a serialized buffer into another buffer is not that easy in Flatbuffers + + osi3::SensorDataBuilder sensor_data_builder(builder); + sensor_data_builder.add_timestamp(timestamp); + //sensor_data_builder.add_version(interface_version); + sensor_data_builder.add_moving_object(detected_moving_object_flatvector); + auto sensor_data = sensor_data_builder.Finish(); + + builder.Finish(sensor_data); + auto uint8_buffer = builder.GetBufferPointer(); + auto size = builder.GetSize(); + std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); + currentOutputBuffer = tmp_buffer; + + normal_log("OSI", "Mapped %d vehicles to output", moving_obj_counter); + + set_fmi_sensor_data_out(); + set_fmi_valid(true); + set_fmi_count(moving_obj_counter); + + } else { + /* We have no valid input, so no valid output */ + normal_log("OSI","No valid input, therefore providing no valid output."); + reset_fmi_sensor_data_out(); + set_fmi_valid(false); + set_fmi_count(0); + } + return fmi2OK; +} + +fmi2Status COSMPDummySensor::doTerm() +{ + DEBUGBREAK(); + + return fmi2OK; +} + +void COSMPDummySensor::doFree() +{ + DEBUGBREAK(); +} + +/* + * Generic C++ Wrapper Code + */ + +COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuType, fmi2String thefmuGUID, fmi2String thefmuResourceLocation, const fmi2CallbackFunctions* thefunctions, fmi2Boolean thevisible, fmi2Boolean theloggingOn) + : instanceName(theinstanceName), + fmuType(thefmuType), + fmuGUID(thefmuGUID), + fmuResourceLocation(thefmuResourceLocation), + functions(*thefunctions), + visible(!!thevisible), + loggingOn(!!theloggingOn), + simulation_started(false) +{ + /*currentOutputBuffer=new string(); + lastOutputBuffer=new string(); + currentConfigRequestBuffer=new string(); + lastConfigRequestBuffer=new string();*/ + loggingCategories.clear(); + loggingCategories.insert("FMI"); + loggingCategories.insert("OSMP"); + loggingCategories.insert("OSI"); +} + +COSMPDummySensor::~COSMPDummySensor() +{ + /*delete currentOutputBuffer; + delete lastOutputBuffer; + delete currentConfigRequestBuffer; + delete lastConfigRequestBuffer;*/ +} + +fmi2Status COSMPDummySensor::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) +{ + fmi_verbose_log("fmi2SetDebugLogging(%s)", theloggingOn ? "true" : "false"); + loggingOn = theloggingOn ? true : false; + if (categories && (nCategories > 0)) { + loggingCategories.clear(); + for (size_t i=0;i", + "FUNCTIONS", visible, loggingOn); + return NULL; + } + + if (myc->doInit() != fmi2OK) { + fmi_verbose_log_global("fmi2Instantiate(\"%s\",%d,\"%s\",\"%s\",\"%s\",%d,%d) = NULL (doInit failure)", + instanceName, fmuType, fmuGUID, + (fmuResourceLocation != NULL) ? fmuResourceLocation : "", + "FUNCTIONS", visible, loggingOn); + delete myc; + return NULL; + } + else { + fmi_verbose_log_global("fmi2Instantiate(\"%s\",%d,\"%s\",\"%s\",\"%s\",%d,%d) = %p", + instanceName, fmuType, fmuGUID, + (fmuResourceLocation != NULL) ? fmuResourceLocation : "", + "FUNCTIONS", visible, loggingOn, myc); + return (fmi2Component)myc; + } +} + +fmi2Status COSMPDummySensor::SetupExperiment(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) +{ + fmi_verbose_log("fmi2SetupExperiment(%d,%g,%g,%d,%g)", toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); + return doStart(toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); +} + +fmi2Status COSMPDummySensor::EnterInitializationMode() +{ + fmi_verbose_log("fmi2EnterInitializationMode()"); + return doEnterInitializationMode(); +} + +fmi2Status COSMPDummySensor::ExitInitializationMode() +{ + fmi_verbose_log("fmi2ExitInitializationMode()"); + simulation_started = true; + return doExitInitializationMode(); +} + +fmi2Status COSMPDummySensor::DoStep(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component) +{ + fmi_verbose_log("fmi2DoStep(%g,%g,%d)", currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); + return doCalc(currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); +} + +fmi2Status COSMPDummySensor::Terminate() +{ + fmi_verbose_log("fmi2Terminate()"); + return doTerm(); +} + +fmi2Status COSMPDummySensor::Reset() +{ + fmi_verbose_log("fmi2Reset()"); + + doFree(); + simulation_started = false; + return doInit(); +} + +void COSMPDummySensor::FreeInstance() +{ + fmi_verbose_log("fmi2FreeInstance()"); + doFree(); +} + +fmi2Status COSMPDummySensor::GetReal(const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]) +{ + fmi_verbose_log("fmi2GetReal(...)"); + for (size_t i = 0; iSetDebugLogging(loggingOn, nCategories, categories); + } + + /* + * Functions for Co-Simulation + */ + FMI2_Export fmi2Component fmi2Instantiate(fmi2String instanceName, + fmi2Type fmuType, + fmi2String fmuGUID, + fmi2String fmuResourceLocation, + const fmi2CallbackFunctions* functions, + fmi2Boolean visible, + fmi2Boolean loggingOn) + { + return COSMPDummySensor::Instantiate(instanceName, fmuType, fmuGUID, fmuResourceLocation, functions, visible, loggingOn); + } + + FMI2_Export fmi2Status fmi2SetupExperiment(fmi2Component c, + fmi2Boolean toleranceDefined, + fmi2Real tolerance, + fmi2Real startTime, + fmi2Boolean stopTimeDefined, + fmi2Real stopTime) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->SetupExperiment(toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); + } + + FMI2_Export fmi2Status fmi2EnterInitializationMode(fmi2Component c) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->EnterInitializationMode(); + } + + FMI2_Export fmi2Status fmi2ExitInitializationMode(fmi2Component c) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->ExitInitializationMode(); + } + + FMI2_Export fmi2Status fmi2DoStep(fmi2Component c, + fmi2Real currentCommunicationPoint, + fmi2Real communicationStepSize, + fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->DoStep(currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); + } + + FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); + return myc->Terminate(); + } + + FMI2_Export fmi2Status fmi2Reset(fmi2Component c) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->Reset(); + } + + FMI2_Export void fmi2FreeInstance(fmi2Component c) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + myc->FreeInstance(); + delete myc; + } + + /* + * Data Exchange Functions + */ + FMI2_Export fmi2Status fmi2GetReal(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->GetReal(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetInteger(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->GetInteger(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetBoolean(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->GetBoolean(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetString(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2String value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->GetString(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetReal(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->SetReal(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetInteger(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->SetInteger(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetBoolean(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->SetBoolean(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetString(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2String value[]) + { + COSMPDummySensor* myc = (COSMPDummySensor*)c; + return myc->SetString(vr, nvr, value); + } + + /* + * Unsupported Features (FMUState, Derivatives, Async DoStep, Status Enquiries) + */ + FMI2_Export fmi2Status fmi2GetFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SetFMUstate(fmi2Component c, fmi2FMUstate FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2FreeFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SerializedFMUstateSize(fmi2Component c, fmi2FMUstate FMUstate, size_t *size) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SerializeFMUstate (fmi2Component c, fmi2FMUstate FMUstate, fmi2Byte serializedState[], size_t size) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2DeSerializeFMUstate (fmi2Component c, const fmi2Byte serializedState[], size_t size, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2GetDirectionalDerivative(fmi2Component c, + const fmi2ValueReference vUnknown_ref[], size_t nUnknown, + const fmi2ValueReference vKnown_ref[] , size_t nKnown, + const fmi2Real dvKnown[], + fmi2Real dvUnknown[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SetRealInputDerivatives(fmi2Component c, + const fmi2ValueReference vr[], + size_t nvr, + const fmi2Integer order[], + const fmi2Real value[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2GetRealOutputDerivatives(fmi2Component c, + const fmi2ValueReference vr[], + size_t nvr, + const fmi2Integer order[], + fmi2Real value[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2CancelStep(fmi2Component c) + { + return fmi2OK; + } + + FMI2_Export fmi2Status fmi2GetStatus(fmi2Component c, const fmi2StatusKind s, fmi2Status* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetRealStatus(fmi2Component c, const fmi2StatusKind s, fmi2Real* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetIntegerStatus(fmi2Component c, const fmi2StatusKind s, fmi2Integer* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetBooleanStatus(fmi2Component c, const fmi2StatusKind s, fmi2Boolean* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetStringStatus(fmi2Component c, const fmi2StatusKind s, fmi2String* value) + { + return fmi2Discard; + } + +} diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.h b/examples/OSMPDummySensor_flat/OSMPDummySensor.h new file mode 100644 index 0000000..78b730c --- /dev/null +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.h @@ -0,0 +1,234 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include "OSMPDummySensorConfig.h" + +using namespace std; + +#ifndef FMU_SHARED_OBJECT +#define FMI2_FUNCTION_PREFIX OSMPDummySensor_ +#endif +#include "fmi2Functions.h" + +/* + * Logging Control + * + * Logging is controlled via three definitions: + * + * - If PRIVATE_LOG_PATH is defined it gives the name of a file + * that is to be used as a private log file. + * - If PUBLIC_LOGGING is defined then we will (also) log to + * the FMI logging facility where appropriate. + * - If VERBOSE_FMI_LOGGING is defined then logging of basic + * FMI calls is enabled, which can get very verbose. + */ + +/* + * Variable Definitions + * + * Define FMI_*_LAST_IDX to the zero-based index of the last variable + * of the given type (0 if no variables of the type exist). This + * ensures proper space allocation, initialisation and handling of + * the given variables in the template code. Optionally you can + * define FMI_TYPENAME_VARNAME_IDX definitions (e.g. FMI_REAL_MYVAR_IDX) + * to refer to individual variables inside your code, or for example + * FMI_REAL_MYARRAY_OFFSET and FMI_REAL_MYARRAY_SIZE definitions for + * array variables. + */ + +/* Boolean Variables */ +#define FMI_BOOLEAN_VALID_IDX 0 +#define FMI_BOOLEAN_LAST_IDX FMI_BOOLEAN_VALID_IDX +#define FMI_BOOLEAN_VARS (FMI_BOOLEAN_LAST_IDX+1) + +/* Integer Variables */ +#define FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX 0 +#define FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX 1 +#define FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX 2 +#define FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX 3 +#define FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX 4 +#define FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX 5 +#define FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX 6 +#define FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX 7 +#define FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX 8 +#define FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX 9 +#define FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX 10 +#define FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX 11 +#define FMI_INTEGER_COUNT_IDX 12 +#define FMI_INTEGER_LAST_IDX FMI_INTEGER_COUNT_IDX +#define FMI_INTEGER_VARS (FMI_INTEGER_LAST_IDX+1) + +/* Real Variables */ +#define FMI_REAL_NOMINAL_RANGE_IDX 0 +#define FMI_REAL_LAST_IDX FMI_REAL_NOMINAL_RANGE_IDX +#define FMI_REAL_VARS (FMI_REAL_LAST_IDX+1) + +/* String Variables */ +#define FMI_STRING_LAST_IDX 0 +#define FMI_STRING_VARS (FMI_STRING_LAST_IDX+1) + +#include +#include +#include +#include +#include + +#undef min +#undef max +#include "osi_sensorview_generated.h" +#include "osi_sensordata_generated.h" +#include "flatbuffers/reflection.h" +#include "flatbuffers/util.h" + +/* FMU Class */ +class COSMPDummySensor { +public: + /* FMI2 Interface mapped to C++ */ + COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuType, fmi2String thefmuGUID, fmi2String thefmuResourceLocation, const fmi2CallbackFunctions* thefunctions, fmi2Boolean thevisible, fmi2Boolean theloggingOn); + ~COSMPDummySensor(); + fmi2Status SetDebugLogging(fmi2Boolean theloggingOn,size_t nCategories, const fmi2String categories[]); + static fmi2Component Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2String fmuGUID, fmi2String fmuResourceLocation, const fmi2CallbackFunctions* functions, fmi2Boolean visible, fmi2Boolean loggingOn); + fmi2Status SetupExperiment(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime); + fmi2Status EnterInitializationMode(); + fmi2Status ExitInitializationMode(); + fmi2Status DoStep(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component); + fmi2Status Terminate(); + fmi2Status Reset(); + void FreeInstance(); + fmi2Status GetReal(const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]); + fmi2Status GetInteger(const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[]); + fmi2Status GetBoolean(const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[]); + fmi2Status GetString(const fmi2ValueReference vr[], size_t nvr, fmi2String value[]); + fmi2Status SetReal(const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[]); + fmi2Status SetInteger(const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[]); + fmi2Status SetBoolean(const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[]); + fmi2Status SetString(const fmi2ValueReference vr[], size_t nvr, const fmi2String value[]); + +protected: + /* Internal Implementation */ + fmi2Status doInit(); + fmi2Status doStart(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime); + fmi2Status doEnterInitializationMode(); + fmi2Status doExitInitializationMode(); + fmi2Status doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component); + fmi2Status doTerm(); + void doFree(); + +protected: + /* Private File-based Logging just for Debugging */ +#ifdef PRIVATE_LOG_PATH + static ofstream private_log_file; +#endif + + static void fmi_verbose_log_global(const char* format, ...) { +#ifdef VERBOSE_FMI_LOGGING +#ifdef PRIVATE_LOG_PATH + va_list ap; + va_start(ap, format); + char buffer[1024]; + if (!private_log_file.is_open()) + private_log_file.open(PRIVATE_LOG_PATH, ios::out | ios::app); + if (private_log_file.is_open()) { +#ifdef _WIN32 + vsnprintf_s(buffer, 1024, format, ap); +#else + vsnprintf(buffer, 1024, format, ap); +#endif + private_log_file << "OSMPDummySensor" << "::Global:FMI: " << buffer << endl; + private_log_file.flush(); + } +#endif +#endif + } + + void internal_log(const char* category, const char* format, va_list arg) + { +#if defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING) + char buffer[1024]; +#ifdef _WIN32 + vsnprintf_s(buffer, 1024, format, arg); +#else + vsnprintf(buffer, 1024, format, arg); +#endif +#ifdef PRIVATE_LOG_PATH + if (!private_log_file.is_open()) + private_log_file.open(PRIVATE_LOG_PATH, ios::out | ios::app); + if (private_log_file.is_open()) { + private_log_file << "OSMPDummySensor" << "::" << instanceName << "<" << ((void*)this) << ">:" << category << ": " << buffer << endl; + private_log_file.flush(); + } +#endif +#ifdef PUBLIC_LOGGING + if (loggingOn && loggingCategories.count(category)) + functions.logger(functions.componentEnvironment,instanceName.c_str(),fmi2OK,category,buffer); +#endif +#endif + } + + void fmi_verbose_log(const char* format, ...) { +#if defined(VERBOSE_FMI_LOGGING) && (defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING)) + va_list ap; + va_start(ap, format); + internal_log("FMI",format,ap); + va_end(ap); +#endif + } + + /* Normal Logging */ + void normal_log(const char* category, const char* format, ...) { +#if defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING) + va_list ap; + va_start(ap, format); + internal_log(category,format,ap); + va_end(ap); +#endif + } + +protected: + /* Members */ + string instanceName; + fmi2Type fmuType; + string fmuGUID; + string fmuResourceLocation; + bool visible; + bool loggingOn; + set loggingCategories; + fmi2CallbackFunctions functions; + fmi2Boolean boolean_vars[FMI_BOOLEAN_VARS]; + fmi2Integer integer_vars[FMI_INTEGER_VARS]; + fmi2Real real_vars[FMI_REAL_VARS]; + string string_vars[FMI_STRING_VARS]; + bool simulation_started; + string currentOutputBuffer; + string lastOutputBuffer; + string currentConfigRequestBuffer; + string lastConfigRequestBuffer; + + /* Simple Accessors */ + fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } + void set_fmi_valid(fmi2Boolean value) { boolean_vars[FMI_BOOLEAN_VALID_IDX]=value; } + fmi2Integer fmi_count() { return integer_vars[FMI_INTEGER_COUNT_IDX]; } + void set_fmi_count(fmi2Integer value) { integer_vars[FMI_INTEGER_COUNT_IDX]=value; } + fmi2Real fmi_nominal_range() { return real_vars[FMI_REAL_NOMINAL_RANGE_IDX]; } + void set_fmi_nominal_range(fmi2Real value) { real_vars[FMI_REAL_NOMINAL_RANGE_IDX]=value; } + + + /* Protocol Buffer Accessors */ + bool get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data); + void set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data); + void reset_fmi_sensor_view_config_request(); + //bool get_fmi_sensor_view_in(osi3::SensorView& data); + const osi3::SensorView* get_fmi_sensor_view_in(); + void set_fmi_sensor_data_out(); + void reset_fmi_sensor_data_out(); + + /* Refreshing of Calculated Parameters */ + void refresh_fmi_sensor_view_config_request(); +}; diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensorConfig.in.h b/examples/OSMPDummySensor_flat/OSMPDummySensorConfig.in.h new file mode 100644 index 0000000..868a5fd --- /dev/null +++ b/examples/OSMPDummySensor_flat/OSMPDummySensorConfig.in.h @@ -0,0 +1,15 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#cmakedefine PUBLIC_LOGGING +#cmakedefine PRIVATE_LOG_PATH "@PRIVATE_LOG_PATH@" +#cmakedefine VERBOSE_FMI_LOGGING +#cmakedefine DEBUG_BREAKS +#define FMU_GUID "@FMUGUID@" diff --git a/examples/OSMPDummySensor_flat/modelDescription.in.xml b/examples/OSMPDummySensor_flat/modelDescription.in.xml new file mode 100644 index 0000000..4b52941 --- /dev/null +++ b/examples/OSMPDummySensor_flat/modelDescription.in.xml @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/OSMPDummySource/CMakeLists.txt b/examples/OSMPDummySource/CMakeLists.txt index 1d53d13..e7c2f48 100644 --- a/examples/OSMPDummySource/CMakeLists.txt +++ b/examples/OSMPDummySource/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySource) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) @@ -28,11 +28,11 @@ add_library(OSMPDummySource SHARED OSMPDummySource.cpp) set_target_properties(OSMPDummySource PROPERTIES PREFIX "") target_compile_definitions(OSMPDummySource PRIVATE "FMU_SHARED_OBJECT") if(LINK_WITH_SHARED_OSI) - target_link_libraries(OSMPDummySource open_simulation_interface_fbs) + target_link_libraries(OSMPDummySource open_simulation_interface) else() target_link_libraries(OSMPDummySource open_simulation_interface_pic) endif() -include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") +include_directories(${CMAKE_CURRENT_BINARY_DIR}) if(WIN32) if(CMAKE_SIZEOF_VOID_P EQUAL 8) diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index b1c16d4..b0e17e0 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -44,14 +44,10 @@ #endif #include -//#include +#include #include #include #include -#include -#include -#include -#include //included for windows compatibility using namespace std; @@ -59,12 +55,6 @@ using namespace std; ofstream COSMPDummySource::private_log_file; #endif -#ifdef _WIN32 -std::string fileName = "C:/tmp/OSMPDummySource_flatbuf_timing"; -#else -std::string fileName = "/tmp/OSMPDummySource_flatbuf_timing"; -#endif - /* * ProtocolBuffer Accessors */ @@ -99,11 +89,9 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) } base; unsigned long long address; } myaddr; - myaddr.address=reinterpret_cast(ptr); hi=myaddr.base.hi; lo=myaddr.base.lo; - #elif PTRDIFF_MAX == INT32_MAX hi=0; lo=reinterpret_cast(ptr); @@ -112,16 +100,13 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) #endif } -//void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) -void COSMPDummySource::set_fmi_sensor_view_out() +void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) { - //data.SerializeToString(currentBuffer); - //encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); - encode_pointer_to_integer(currentBuffer.data(), integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX], integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); - integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer.length(); - normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); - std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); - //swap(currentBuffer,lastBuffer); + data.SerializeToString(currentBuffer); + encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer->length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer->data()); + swap(currentBuffer,lastBuffer); } void COSMPDummySource::reset_fmi_sensor_view_out() @@ -201,7 +186,8 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - flatbuffers::FlatBufferBuilder builder(1024); + + osi3::SensorView currentOut; double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating SensorView at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); @@ -210,93 +196,63 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; static double source_x_offsets[10] = { 0.0, 40.0, 100.0, 100.0, 0.0, 150.0, 5.0, 45.0, 85.0, 125.0 }; static double source_x_speeds[10] = { 29.0, 30.0, 31.0, 25.0, 26.0, 28.0, 20.0, 22.0, 22.5, 23.0 }; - static osi3::MovingObject_::VehicleClassification_::Type source_veh_types[10] = { - osi3::MovingObject_::VehicleClassification_::Type::TYPE_MEDIUM_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_SMALL_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_COMPACT_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_DELIVERY_VAN, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_LUXURY_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_MEDIUM_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_COMPACT_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_SMALL_CAR, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_MOTORBIKE, - osi3::MovingObject_::VehicleClassification_::Type::TYPE_BUS }; - - std::vector> moving_object_vector; + static osi3::MovingObject_VehicleClassification_Type source_veh_types[10] = { + osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN, + osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE, + osi3::MovingObject_VehicleClassification_Type_TYPE_BUS }; + + currentOut.Clear(); + currentOut.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); + currentOut.mutable_sensor_id()->set_value(10000); + currentOut.mutable_host_vehicle_id()->set_value(14); + osi3::GroundTruth *currentGT = currentOut.mutable_global_ground_truth(); + currentOut.mutable_timestamp()->set_seconds((long long int)floor(time)); + currentOut.mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); + currentGT->mutable_timestamp()->set_seconds((long long int)floor(time)); + currentGT->mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); + currentGT->mutable_host_vehicle_id()->set_value(14); + // Vehicles for (unsigned int i=0;i<10;i++) { - osi3::IdentifierBuilder id_builder(builder); - id_builder.add_value(10+i); - auto moving_obj_id = id_builder.Finish(); - - osi3::MovingObject_::VehicleClassificationBuilder vehicle_classification_builder(builder); - //vehicle_classification_builder.add_type(source_veh_types[i]); //todo: vehicle classifications are wrong in headers due to namespace conflict -> confused with moving object type - auto vehicle_classification = vehicle_classification_builder.Finish(); - - auto bbcenter_to_rear = osi3::CreateVector3d(builder, 1.0, 0.0, -0.3); - osi3::MovingObject_::VehicleAttributesBuilder vehicle_attributes_builder(builder); - vehicle_attributes_builder.add_bbcenter_to_rear(bbcenter_to_rear); - auto vehicle_attributes = vehicle_attributes_builder.Finish(); - - auto dimension = osi3::CreateDimension3d(builder, 5.0, 2.0, 1.5); - auto position = osi3::CreateVector3d(builder, source_x_offsets[i]+time*source_x_speeds[i], source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25, 0.0); - auto velocity = osi3::CreateVector3d(builder, source_x_speeds[i], cos(time/source_x_speeds[i])*0.25/source_x_speeds[i], 0.0); - auto acceleration = osi3::CreateVector3d(builder, 0.0, -sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i]), 0.0); - auto orientation = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); - auto orientation_rate = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); - osi3::BaseMovingBuilder base_moving_builder(builder); - base_moving_builder.add_dimension(dimension); - base_moving_builder.add_position(position); - base_moving_builder.add_velocity(velocity); - base_moving_builder.add_acceleration(acceleration); - base_moving_builder.add_orientation(orientation); - base_moving_builder.add_orientation_rate(orientation_rate); - auto base_moving = base_moving_builder.Finish(); - - osi3::MovingObjectBuilder moving_object_builder(builder); - moving_object_builder.add_id(moving_obj_id); - //moving_object_builder.add_type(osi3::MovingObject_::Type::TYPE_VEHICLE); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused - moving_object_builder.add_vehicle_classification(vehicle_classification); - moving_object_builder.add_vehicle_attributes(vehicle_attributes); - moving_object_builder.add_base(base_moving); - auto current_moving_object = moving_object_builder.Finish(); - moving_object_vector.push_back(current_moving_object); - - auto moving_object = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - current_moving_object.o); - normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,moving_object->id()->value(),moving_object->base()->position()->x(),moving_object->base()->position()->y(),moving_object->base()->position()->z(),moving_object->base()->velocity()->x(),moving_object->base()->velocity()->y(),moving_object->base()->velocity()->z()); - } - auto moving_object_flatvector = builder.CreateVector(moving_object_vector); - - auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); - osi3::IdentifierBuilder host_vehicle_id_builder(builder); - host_vehicle_id_builder.add_value(14); - auto host_vehicle_id = host_vehicle_id_builder.Finish(); - osi3::GroundTruthBuilder ground_truth_builder(builder); - ground_truth_builder.add_timestamp(timestamp); - ground_truth_builder.add_host_vehicle_id(host_vehicle_id); - ground_truth_builder.add_moving_object(moving_object_flatvector); - auto ground_truth = ground_truth_builder.Finish(); - - auto sensor_id = osi3::CreateIdentifier(builder, 10000); - - osi3::SensorViewBuilder sensor_view_builder(builder); - //sensor_view_builder.add_version(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); //todo: the used Protobuf FileOptions do not exist in Flatbuffers - sensor_view_builder.add_sensor_id(sensor_id); - sensor_view_builder.add_host_vehicle_id(host_vehicle_id); - sensor_view_builder.add_timestamp(timestamp); - sensor_view_builder.add_global_ground_truth(ground_truth); - auto sensor_view = sensor_view_builder.Finish(); - - builder.Finish(sensor_view); - auto uint8_buffer = builder.GetBufferPointer(); - auto size = builder.GetSize(); - std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); - currentBuffer = tmp_buffer; - - set_fmi_sensor_view_out(); + osi3::MovingObject *veh = currentGT->add_moving_object(); + veh->mutable_id()->set_value(10+i); + veh->set_type(osi3::MovingObject_Type_TYPE_VEHICLE); + auto vehclass = veh->mutable_vehicle_classification(); + vehclass->set_type(source_veh_types[i]); + auto vehlights = vehclass->mutable_light_state(); + vehlights->set_indicator_state(osi3::MovingObject_VehicleClassification_LightState_IndicatorState_INDICATOR_STATE_OFF); + vehlights->set_brake_light_state(osi3::MovingObject_VehicleClassification_LightState_BrakeLightState_BRAKE_LIGHT_STATE_OFF); + veh->mutable_base()->mutable_dimension()->set_height(1.5); + veh->mutable_base()->mutable_dimension()->set_width(2.0); + veh->mutable_base()->mutable_dimension()->set_length(5.0); + veh->mutable_base()->mutable_position()->set_x(source_x_offsets[i]+time*source_x_speeds[i]); + veh->mutable_base()->mutable_position()->set_y(source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25); + veh->mutable_base()->mutable_position()->set_z(0.0); + veh->mutable_base()->mutable_velocity()->set_x(source_x_speeds[i]); + veh->mutable_base()->mutable_velocity()->set_y(cos(time/source_x_speeds[i])*0.25/source_x_speeds[i]); + veh->mutable_base()->mutable_velocity()->set_z(0.0); + veh->mutable_base()->mutable_acceleration()->set_x(0.0); + veh->mutable_base()->mutable_acceleration()->set_y(-sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i])); + veh->mutable_base()->mutable_acceleration()->set_z(0.0); + veh->mutable_base()->mutable_orientation()->set_pitch(0.0); + veh->mutable_base()->mutable_orientation()->set_roll(0.0); + veh->mutable_base()->mutable_orientation()->set_yaw(0.0); + veh->mutable_base()->mutable_orientation_rate()->set_pitch(0.0); + veh->mutable_base()->mutable_orientation_rate()->set_roll(0.0); + veh->mutable_base()->mutable_orientation_rate()->set_yaw(0.0); + normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,veh->id().value(),veh->base().position().x(),veh->base().position().y(),veh->base().position().z(),veh->base().velocity().x(),veh->base().velocity().y(),veh->base().velocity().z()); + } + + set_fmi_sensor_view_out(currentOut); set_fmi_valid(true); - set_fmi_count((int)moving_object_vector.size()); - + set_fmi_count(currentGT->moving_object_size()); return fmi2OK; } @@ -324,10 +280,8 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy visible(!!thevisible), loggingOn(!!theloggingOn) { - //currentBuffer = new string(); - //currentBuffer = new uint8_t(); - //lastBuffer = new string(); - //lastBuffer = new uint8_t(); + currentBuffer = new string(); + lastBuffer = new string(); loggingCategories.clear(); loggingCategories.insert("FMI"); loggingCategories.insert("OSMP"); @@ -336,8 +290,8 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy COSMPDummySource::~COSMPDummySource() { - //delete currentBuffer; - //delete lastBuffer; + delete currentBuffer; + delete lastBuffer; } fmi2Status COSMPDummySource::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) @@ -603,14 +557,6 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySource* myc = (COSMPDummySource*)c; - std::ofstream logFile; - logFile.open(fileName, std::ios_base::app); - logFile << std::endl << "\t\t\t]" << std::endl; - logFile << "\t\t}" << std::endl; - logFile << "\t]" << std::endl; - logFile << "}" << std::endl; - logFile.close(); - return myc->Terminate(); } diff --git a/examples/OSMPDummySource/OSMPDummySource.h b/examples/OSMPDummySource/OSMPDummySource.h index 4f987b9..230a6e3 100644 --- a/examples/OSMPDummySource/OSMPDummySource.h +++ b/examples/OSMPDummySource/OSMPDummySource.h @@ -72,7 +72,7 @@ using namespace std; #undef min #undef max -#include "osi_sensorview_generated.h" +#include "osi_sensorview.pb.h" /* FMU Class */ class COSMPDummySource { @@ -192,20 +192,16 @@ class COSMPDummySource { fmi2Integer integer_vars[FMI_INTEGER_VARS]; fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; - //string* currentBuffer; - string currentBuffer; - //string* lastBuffer; - string lastBuffer; + string* currentBuffer; + string* lastBuffer; - /* Simple Accessors */ + /* Simple Accessors */ fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } void set_fmi_valid(fmi2Boolean value) { boolean_vars[FMI_BOOLEAN_VALID_IDX]=value; } fmi2Integer fmi_count() { return integer_vars[FMI_INTEGER_COUNT_IDX]; } void set_fmi_count(fmi2Integer value) { integer_vars[FMI_INTEGER_COUNT_IDX]=value; } /* Protocol Buffer Accessors */ - //void set_fmi_sensor_view_out(const osi3::SensorView& data); - void set_fmi_sensor_view_out(); + void set_fmi_sensor_view_out(const osi3::SensorView& data); void reset_fmi_sensor_view_out(); - }; diff --git a/examples/OSMPDummySource_flat/CMakeLists.txt b/examples/OSMPDummySource_flat/CMakeLists.txt new file mode 100644 index 0000000..1d53d13 --- /dev/null +++ b/examples/OSMPDummySource_flat/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(OSMPDummySource) + +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") +set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") +if(WIN32) + set(PRIVATE_LOG_PATH_SOURCE "C:/TEMP/OSMPDummySourceLog.log" CACHE FILEPATH "Path to write private log file to") +else() + set(PRIVATE_LOG_PATH_SOURCE "/tmp/OSMPDummySourceLog.log" CACHE FILEPATH "Path to write private log file to") +endif() +if(PRIVATE_LOGGING) + file(TO_NATIVE_PATH ${PRIVATE_LOG_PATH_SOURCE} PRIVATE_LOG_PATH_SOURCE_NATIVE) + string(REPLACE "\\" "\\\\" PRIVATE_LOG_PATH ${PRIVATE_LOG_PATH_SOURCE_NATIVE}) +endif() +set(VERBOSE_FMI_LOGGING OFF CACHE BOOL "Enable detailed FMI function logging") +set(DEBUG_BREAKS OFF CACHE BOOL "Enable debugger traps for debug builds of FMU") + +string(TIMESTAMP FMUTIMESTAMP UTC) +string(MD5 FMUGUID modelDescription.in.xml) +configure_file(modelDescription.in.xml modelDescription.xml @ONLY) +configure_file(OSMPDummySourceConfig.in.h OSMPDummySourceConfig.h) + +find_package(Protobuf 2.6.1 REQUIRED) +add_library(OSMPDummySource SHARED OSMPDummySource.cpp) +set_target_properties(OSMPDummySource PROPERTIES PREFIX "") +target_compile_definitions(OSMPDummySource PRIVATE "FMU_SHARED_OBJECT") +if(LINK_WITH_SHARED_OSI) + target_link_libraries(OSMPDummySource open_simulation_interface_fbs) +else() + target_link_libraries(OSMPDummySource open_simulation_interface_pic) +endif() +include_directories("${CMAKE_CURRENT_BINARY_DIR}" "${CMAKE_CURRENT_BINARY_DIR}/include") + +if(WIN32) + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "win64") + else() + set(FMI_BINARIES_PLATFORM "win32") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Linux") + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "linux64") + else() + set(FMI_BINARIES_PLATFORM "linux32") + endif() +elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") + if(CMAKE_SIZEOF_VOID_P EQUAL 8) + set(FMI_BINARIES_PLATFORM "darwin64") + else() + set(FMI_BINARIES_PLATFORM "darwin32") + endif() +endif() + +add_custom_command(TARGET OSMPDummySource + POST_BUILD + COMMAND ${CMAKE_COMMAND} -E remove_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" + COMMAND ${CMAKE_COMMAND} -E make_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources" + COMMAND ${CMAKE_COMMAND} -E make_directory "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_BINARY_DIR}/modelDescription.xml" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/OSMPDummySource.cpp" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/OSMPDummySource.h" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/" + COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_BINARY_DIR}/OSMPDummySourceConfig.h" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources/OSMPDummySourceConfig.h" + COMMAND ${CMAKE_COMMAND} -E copy $ $<$:$<$:$>> "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}" + COMMAND ${CMAKE_COMMAND} -E chdir "${CMAKE_CURRENT_BINARY_DIR}/buildfmu" ${CMAKE_COMMAND} -E tar "cfv" "../OSMPDummySource.fmu" --format=zip "modelDescription.xml" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/sources" "${CMAKE_CURRENT_BINARY_DIR}/buildfmu/binaries/${FMI_BINARIES_PLATFORM}") diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp new file mode 100644 index 0000000..b1c16d4 --- /dev/null +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -0,0 +1,771 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include "OSMPDummySource.h" + +/* + * Debug Breaks + * + * If you define DEBUG_BREAKS the FMU will automatically break + * into an attached Debugger on all major computation functions. + * Note that the FMU is likely to break all environments if no + * Debugger is actually attached when the breaks are triggered. + */ +#if defined(DEBUG_BREAKS) && !defined(NDEBUG) +#if defined(__has_builtin) && !defined(__ibmxl__) +#if __has_builtin(__builtin_debugtrap) +#define DEBUGBREAK() __builtin_debugtrap() +#elif __has_builtin(__debugbreak) +#define DEBUGBREAK() __debugbreak() +#endif +#endif +#if !defined(DEBUGBREAK) +#if defined(_MSC_VER) || defined(__INTEL_COMPILER) +#include +#define DEBUGBREAK() __debugbreak() +#else +#include +#if defined(SIGTRAP) +#define DEBUGBREAK() raise(SIGTRAP) +#else +#define DEBUGBREAK() raise(SIGABRT) +#endif +#endif +#endif +#else +#define DEBUGBREAK() +#endif + +#include +//#include +#include +#include +#include +#include +#include +#include +#include //included for windows compatibility + +using namespace std; + +#ifdef PRIVATE_LOG_PATH +ofstream COSMPDummySource::private_log_file; +#endif + +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySource_flatbuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySource_flatbuf_timing"; +#endif + +/* + * ProtocolBuffer Accessors + */ + +void* decode_integer_to_pointer(fmi2Integer hi,fmi2Integer lo) +{ +#if PTRDIFF_MAX == INT64_MAX + union addrconv { + struct { + int lo; + int hi; + } base; + unsigned long long address; + } myaddr; + myaddr.base.lo=lo; + myaddr.base.hi=hi; + return reinterpret_cast(myaddr.address); +#elif PTRDIFF_MAX == INT32_MAX + return reinterpret_cast(lo); +#else +#error "Cannot determine 32bit or 64bit environment!" +#endif +} + +void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) +{ +#if PTRDIFF_MAX == INT64_MAX + union addrconv { + struct { + int lo; + int hi; + } base; + unsigned long long address; + } myaddr; + + myaddr.address=reinterpret_cast(ptr); + hi=myaddr.base.hi; + lo=myaddr.base.lo; + +#elif PTRDIFF_MAX == INT32_MAX + hi=0; + lo=reinterpret_cast(ptr); +#else +#error "Cannot determine 32bit or 64bit environment!" +#endif +} + +//void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) +void COSMPDummySource::set_fmi_sensor_view_out() +{ + //data.SerializeToString(currentBuffer); + //encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); + encode_pointer_to_integer(currentBuffer.data(), integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX], integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer.length(); + normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); + std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); + //swap(currentBuffer,lastBuffer); +} + +void COSMPDummySource::reset_fmi_sensor_view_out() +{ + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=0; + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX]=0; + integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]=0; +} + +/* + * Actual Core Content + */ + +fmi2Status COSMPDummySource::doInit() +{ + DEBUGBREAK(); + + /* Booleans */ + for (int i = 0; i> moving_object_vector; + // Vehicles + for (unsigned int i=0;i<10;i++) { + osi3::IdentifierBuilder id_builder(builder); + id_builder.add_value(10+i); + auto moving_obj_id = id_builder.Finish(); + + osi3::MovingObject_::VehicleClassificationBuilder vehicle_classification_builder(builder); + //vehicle_classification_builder.add_type(source_veh_types[i]); //todo: vehicle classifications are wrong in headers due to namespace conflict -> confused with moving object type + auto vehicle_classification = vehicle_classification_builder.Finish(); + + auto bbcenter_to_rear = osi3::CreateVector3d(builder, 1.0, 0.0, -0.3); + osi3::MovingObject_::VehicleAttributesBuilder vehicle_attributes_builder(builder); + vehicle_attributes_builder.add_bbcenter_to_rear(bbcenter_to_rear); + auto vehicle_attributes = vehicle_attributes_builder.Finish(); + + auto dimension = osi3::CreateDimension3d(builder, 5.0, 2.0, 1.5); + auto position = osi3::CreateVector3d(builder, source_x_offsets[i]+time*source_x_speeds[i], source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25, 0.0); + auto velocity = osi3::CreateVector3d(builder, source_x_speeds[i], cos(time/source_x_speeds[i])*0.25/source_x_speeds[i], 0.0); + auto acceleration = osi3::CreateVector3d(builder, 0.0, -sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i]), 0.0); + auto orientation = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); + auto orientation_rate = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); + osi3::BaseMovingBuilder base_moving_builder(builder); + base_moving_builder.add_dimension(dimension); + base_moving_builder.add_position(position); + base_moving_builder.add_velocity(velocity); + base_moving_builder.add_acceleration(acceleration); + base_moving_builder.add_orientation(orientation); + base_moving_builder.add_orientation_rate(orientation_rate); + auto base_moving = base_moving_builder.Finish(); + + osi3::MovingObjectBuilder moving_object_builder(builder); + moving_object_builder.add_id(moving_obj_id); + //moving_object_builder.add_type(osi3::MovingObject_::Type::TYPE_VEHICLE); //todo: vehicle types are wrong in headers due to namespace conflict -> stationary and moving are confused + moving_object_builder.add_vehicle_classification(vehicle_classification); + moving_object_builder.add_vehicle_attributes(vehicle_attributes); + moving_object_builder.add_base(base_moving); + auto current_moving_object = moving_object_builder.Finish(); + moving_object_vector.push_back(current_moving_object); + + auto moving_object = reinterpret_cast(builder.GetCurrentBufferPointer() + builder.GetSize() - current_moving_object.o); + normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,moving_object->id()->value(),moving_object->base()->position()->x(),moving_object->base()->position()->y(),moving_object->base()->position()->z(),moving_object->base()->velocity()->x(),moving_object->base()->velocity()->y(),moving_object->base()->velocity()->z()); + } + auto moving_object_flatvector = builder.CreateVector(moving_object_vector); + + auto timestamp = osi3::CreateTimestamp(builder, (int64_t)floor(time), (int)((time - floor(time))*1000000000.0)); + osi3::IdentifierBuilder host_vehicle_id_builder(builder); + host_vehicle_id_builder.add_value(14); + auto host_vehicle_id = host_vehicle_id_builder.Finish(); + osi3::GroundTruthBuilder ground_truth_builder(builder); + ground_truth_builder.add_timestamp(timestamp); + ground_truth_builder.add_host_vehicle_id(host_vehicle_id); + ground_truth_builder.add_moving_object(moving_object_flatvector); + auto ground_truth = ground_truth_builder.Finish(); + + auto sensor_id = osi3::CreateIdentifier(builder, 10000); + + osi3::SensorViewBuilder sensor_view_builder(builder); + //sensor_view_builder.add_version(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); //todo: the used Protobuf FileOptions do not exist in Flatbuffers + sensor_view_builder.add_sensor_id(sensor_id); + sensor_view_builder.add_host_vehicle_id(host_vehicle_id); + sensor_view_builder.add_timestamp(timestamp); + sensor_view_builder.add_global_ground_truth(ground_truth); + auto sensor_view = sensor_view_builder.Finish(); + + builder.Finish(sensor_view); + auto uint8_buffer = builder.GetBufferPointer(); + auto size = builder.GetSize(); + std::string tmp_buffer(reinterpret_cast(uint8_buffer), size); + currentBuffer = tmp_buffer; + + set_fmi_sensor_view_out(); + set_fmi_valid(true); + set_fmi_count((int)moving_object_vector.size()); + + return fmi2OK; +} + +fmi2Status COSMPDummySource::doTerm() +{ + DEBUGBREAK(); + return fmi2OK; +} + +void COSMPDummySource::doFree() +{ + DEBUGBREAK(); +} + +/* + * Generic C++ Wrapper Code + */ + +COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuType, fmi2String thefmuGUID, fmi2String thefmuResourceLocation, const fmi2CallbackFunctions* thefunctions, fmi2Boolean thevisible, fmi2Boolean theloggingOn) + : instanceName(theinstanceName), + fmuType(thefmuType), + fmuGUID(thefmuGUID), + fmuResourceLocation(thefmuResourceLocation), + functions(*thefunctions), + visible(!!thevisible), + loggingOn(!!theloggingOn) +{ + //currentBuffer = new string(); + //currentBuffer = new uint8_t(); + //lastBuffer = new string(); + //lastBuffer = new uint8_t(); + loggingCategories.clear(); + loggingCategories.insert("FMI"); + loggingCategories.insert("OSMP"); + loggingCategories.insert("OSI"); +} + +COSMPDummySource::~COSMPDummySource() +{ + //delete currentBuffer; + //delete lastBuffer; +} + +fmi2Status COSMPDummySource::SetDebugLogging(fmi2Boolean theloggingOn, size_t nCategories, const fmi2String categories[]) +{ + fmi_verbose_log("fmi2SetDebugLogging(%s)", theloggingOn ? "true" : "false"); + loggingOn = theloggingOn ? true : false; + if (categories && (nCategories > 0)) { + loggingCategories.clear(); + for (size_t i=0;i", + "FUNCTIONS", visible, loggingOn); + return NULL; + } + + if (myc->doInit() != fmi2OK) { + fmi_verbose_log_global("fmi2Instantiate(\"%s\",%d,\"%s\",\"%s\",\"%s\",%d,%d) = NULL (doInit failure)", + instanceName, fmuType, fmuGUID, + (fmuResourceLocation != NULL) ? fmuResourceLocation : "", + "FUNCTIONS", visible, loggingOn); + delete myc; + return NULL; + } + else { + fmi_verbose_log_global("fmi2Instantiate(\"%s\",%d,\"%s\",\"%s\",\"%s\",%d,%d) = %p", + instanceName, fmuType, fmuGUID, + (fmuResourceLocation != NULL) ? fmuResourceLocation : "", + "FUNCTIONS", visible, loggingOn, myc); + return (fmi2Component)myc; + } +} + +fmi2Status COSMPDummySource::SetupExperiment(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) +{ + fmi_verbose_log("fmi2SetupExperiment(%d,%g,%g,%d,%g)", toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); + return doStart(toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); +} + +fmi2Status COSMPDummySource::EnterInitializationMode() +{ + fmi_verbose_log("fmi2EnterInitializationMode()"); + return doEnterInitializationMode(); +} + +fmi2Status COSMPDummySource::ExitInitializationMode() +{ + fmi_verbose_log("fmi2ExitInitializationMode()"); + return doExitInitializationMode(); +} + +fmi2Status COSMPDummySource::DoStep(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component) +{ + fmi_verbose_log("fmi2DoStep(%g,%g,%d)", currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); + return doCalc(currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); +} + +fmi2Status COSMPDummySource::Terminate() +{ + fmi_verbose_log("fmi2Terminate()"); + return doTerm(); +} + +fmi2Status COSMPDummySource::Reset() +{ + fmi_verbose_log("fmi2Reset()"); + + doFree(); + return doInit(); +} + +void COSMPDummySource::FreeInstance() +{ + fmi_verbose_log("fmi2FreeInstance()"); + doFree(); +} + +fmi2Status COSMPDummySource::GetReal(const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]) +{ + fmi_verbose_log("fmi2GetReal(...)"); + for (size_t i = 0; iSetDebugLogging(loggingOn, nCategories, categories); + } + + /* + * Functions for Co-Simulation + */ + FMI2_Export fmi2Component fmi2Instantiate(fmi2String instanceName, + fmi2Type fmuType, + fmi2String fmuGUID, + fmi2String fmuResourceLocation, + const fmi2CallbackFunctions* functions, + fmi2Boolean visible, + fmi2Boolean loggingOn) + { + return COSMPDummySource::Instantiate(instanceName, fmuType, fmuGUID, fmuResourceLocation, functions, visible, loggingOn); + } + + FMI2_Export fmi2Status fmi2SetupExperiment(fmi2Component c, + fmi2Boolean toleranceDefined, + fmi2Real tolerance, + fmi2Real startTime, + fmi2Boolean stopTimeDefined, + fmi2Real stopTime) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->SetupExperiment(toleranceDefined, tolerance, startTime, stopTimeDefined, stopTime); + } + + FMI2_Export fmi2Status fmi2EnterInitializationMode(fmi2Component c) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->EnterInitializationMode(); + } + + FMI2_Export fmi2Status fmi2ExitInitializationMode(fmi2Component c) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->ExitInitializationMode(); + } + + FMI2_Export fmi2Status fmi2DoStep(fmi2Component c, + fmi2Real currentCommunicationPoint, + fmi2Real communicationStepSize, + fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->DoStep(currentCommunicationPoint, communicationStepSize, noSetFMUStatePriorToCurrentPointfmi2Component); + } + + FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); + + return myc->Terminate(); + } + + FMI2_Export fmi2Status fmi2Reset(fmi2Component c) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->Reset(); + } + + FMI2_Export void fmi2FreeInstance(fmi2Component c) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + myc->FreeInstance(); + delete myc; + } + + /* + * Data Exchange Functions + */ + FMI2_Export fmi2Status fmi2GetReal(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->GetReal(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetInteger(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->GetInteger(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetBoolean(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->GetBoolean(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2GetString(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, fmi2String value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->GetString(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetReal(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->SetReal(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetInteger(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->SetInteger(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetBoolean(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->SetBoolean(vr, nvr, value); + } + + FMI2_Export fmi2Status fmi2SetString(fmi2Component c, const fmi2ValueReference vr[], size_t nvr, const fmi2String value[]) + { + COSMPDummySource* myc = (COSMPDummySource*)c; + return myc->SetString(vr, nvr, value); + } + + /* + * Unsupported Features (FMUState, Derivatives, Async DoStep, Status Enquiries) + */ + FMI2_Export fmi2Status fmi2GetFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SetFMUstate(fmi2Component c, fmi2FMUstate FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2FreeFMUstate(fmi2Component c, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SerializedFMUstateSize(fmi2Component c, fmi2FMUstate FMUstate, size_t *size) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SerializeFMUstate (fmi2Component c, fmi2FMUstate FMUstate, fmi2Byte serializedState[], size_t size) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2DeSerializeFMUstate (fmi2Component c, const fmi2Byte serializedState[], size_t size, fmi2FMUstate* FMUstate) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2GetDirectionalDerivative(fmi2Component c, + const fmi2ValueReference vUnknown_ref[], size_t nUnknown, + const fmi2ValueReference vKnown_ref[] , size_t nKnown, + const fmi2Real dvKnown[], + fmi2Real dvUnknown[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2SetRealInputDerivatives(fmi2Component c, + const fmi2ValueReference vr[], + size_t nvr, + const fmi2Integer order[], + const fmi2Real value[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2GetRealOutputDerivatives(fmi2Component c, + const fmi2ValueReference vr[], + size_t nvr, + const fmi2Integer order[], + fmi2Real value[]) + { + return fmi2Error; + } + + FMI2_Export fmi2Status fmi2CancelStep(fmi2Component c) + { + return fmi2OK; + } + + FMI2_Export fmi2Status fmi2GetStatus(fmi2Component c, const fmi2StatusKind s, fmi2Status* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetRealStatus(fmi2Component c, const fmi2StatusKind s, fmi2Real* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetIntegerStatus(fmi2Component c, const fmi2StatusKind s, fmi2Integer* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetBooleanStatus(fmi2Component c, const fmi2StatusKind s, fmi2Boolean* value) + { + return fmi2Discard; + } + + FMI2_Export fmi2Status fmi2GetStringStatus(fmi2Component c, const fmi2StatusKind s, fmi2String* value) + { + return fmi2Discard; + } + +} diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.h b/examples/OSMPDummySource_flat/OSMPDummySource.h new file mode 100644 index 0000000..4f987b9 --- /dev/null +++ b/examples/OSMPDummySource_flat/OSMPDummySource.h @@ -0,0 +1,211 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#include "OSMPDummySourceConfig.h" + +using namespace std; + +#ifndef FMU_SHARED_OBJECT +#define FMI2_FUNCTION_PREFIX OSMPDummySource_ +#endif +#include "fmi2Functions.h" + +/* + * Logging Control + * + * Logging is controlled via three definitions: + * + * - If PRIVATE_LOG_PATH is defined it gives the name of a file + * that is to be used as a private log file. + * - If PUBLIC_LOGGING is defined then we will (also) log to + * the FMI logging facility where appropriate. + * - If VERBOSE_FMI_LOGGING is defined then logging of basic + * FMI calls is enabled, which can get very verbose. + */ + +/* + * Variable Definitions + * + * Define FMI_*_LAST_IDX to the zero-based index of the last variable + * of the given type (0 if no variables of the type exist). This + * ensures proper space allocation, initialisation and handling of + * the given variables in the template code. Optionally you can + * define FMI_TYPENAME_VARNAME_IDX definitions (e.g. FMI_REAL_MYVAR_IDX) + * to refer to individual variables inside your code, or for example + * FMI_REAL_MYARRAY_OFFSET and FMI_REAL_MYARRAY_SIZE definitions for + * array variables. + */ + +/* Boolean Variables */ +#define FMI_BOOLEAN_VALID_IDX 0 +#define FMI_BOOLEAN_LAST_IDX FMI_BOOLEAN_VALID_IDX +#define FMI_BOOLEAN_VARS (FMI_BOOLEAN_LAST_IDX+1) + +/* Integer Variables */ +#define FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX 0 +#define FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX 1 +#define FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX 2 +#define FMI_INTEGER_COUNT_IDX 3 +#define FMI_INTEGER_LAST_IDX FMI_INTEGER_COUNT_IDX +#define FMI_INTEGER_VARS (FMI_INTEGER_LAST_IDX+1) + +/* Real Variables */ +#define FMI_REAL_LAST_IDX 0 +#define FMI_REAL_VARS (FMI_REAL_LAST_IDX+1) + +/* String Variables */ +#define FMI_STRING_LAST_IDX 0 +#define FMI_STRING_VARS (FMI_STRING_LAST_IDX+1) + +#include +#include +#include +#include +#include + +#undef min +#undef max +#include "osi_sensorview_generated.h" + +/* FMU Class */ +class COSMPDummySource { +public: + /* FMI2 Interface mapped to C++ */ + COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuType, fmi2String thefmuGUID, fmi2String thefmuResourceLocation, const fmi2CallbackFunctions* thefunctions, fmi2Boolean thevisible, fmi2Boolean theloggingOn); + ~COSMPDummySource(); + fmi2Status SetDebugLogging(fmi2Boolean theloggingOn,size_t nCategories, const fmi2String categories[]); + static fmi2Component Instantiate(fmi2String instanceName, fmi2Type fmuType, fmi2String fmuGUID, fmi2String fmuResourceLocation, const fmi2CallbackFunctions* functions, fmi2Boolean visible, fmi2Boolean loggingOn); + fmi2Status SetupExperiment(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime); + fmi2Status EnterInitializationMode(); + fmi2Status ExitInitializationMode(); + fmi2Status DoStep(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component); + fmi2Status Terminate(); + fmi2Status Reset(); + void FreeInstance(); + fmi2Status GetReal(const fmi2ValueReference vr[], size_t nvr, fmi2Real value[]); + fmi2Status GetInteger(const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[]); + fmi2Status GetBoolean(const fmi2ValueReference vr[], size_t nvr, fmi2Boolean value[]); + fmi2Status GetString(const fmi2ValueReference vr[], size_t nvr, fmi2String value[]); + fmi2Status SetReal(const fmi2ValueReference vr[], size_t nvr, const fmi2Real value[]); + fmi2Status SetInteger(const fmi2ValueReference vr[], size_t nvr, const fmi2Integer value[]); + fmi2Status SetBoolean(const fmi2ValueReference vr[], size_t nvr, const fmi2Boolean value[]); + fmi2Status SetString(const fmi2ValueReference vr[], size_t nvr, const fmi2String value[]); + +protected: + /* Internal Implementation */ + fmi2Status doInit(); + fmi2Status doStart(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime); + fmi2Status doEnterInitializationMode(); + fmi2Status doExitInitializationMode(); + fmi2Status doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component); + fmi2Status doTerm(); + void doFree(); + +protected: + /* Private File-based Logging just for Debugging */ +#ifdef PRIVATE_LOG_PATH + static ofstream private_log_file; +#endif + + static void fmi_verbose_log_global(const char* format, ...) { +#ifdef VERBOSE_FMI_LOGGING +#ifdef PRIVATE_LOG_PATH + va_list ap; + va_start(ap, format); + char buffer[1024]; + if (!private_log_file.is_open()) + private_log_file.open(PRIVATE_LOG_PATH, ios::out | ios::app); + if (private_log_file.is_open()) { +#ifdef _WIN32 + vsnprintf_s(buffer, 1024, format, ap); +#else + vsnprintf(buffer, 1024, format, ap); +#endif + private_log_file << "OSMPDummySource" << "::Global:FMI: " << buffer << endl; + private_log_file.flush(); + } +#endif +#endif + } + + void internal_log(const char* category, const char* format, va_list arg) + { +#if defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING) + char buffer[1024]; +#ifdef _WIN32 + vsnprintf_s(buffer, 1024, format, arg); +#else + vsnprintf(buffer, 1024, format, arg); +#endif +#ifdef PRIVATE_LOG_PATH + if (!private_log_file.is_open()) + private_log_file.open(PRIVATE_LOG_PATH, ios::out | ios::app); + if (private_log_file.is_open()) { + private_log_file << "OSMPDummySource" << "::" << instanceName << "<" << ((void*)this) << ">:" << category << ": " << buffer << endl; + private_log_file.flush(); + } +#endif +#ifdef PUBLIC_LOGGING + if (loggingOn && loggingCategories.count(category)) + functions.logger(functions.componentEnvironment,instanceName.c_str(),fmi2OK,category,buffer); +#endif +#endif + } + + void fmi_verbose_log(const char* format, ...) { +#if defined(VERBOSE_FMI_LOGGING) && (defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING)) + va_list ap; + va_start(ap, format); + internal_log("FMI",format,ap); + va_end(ap); +#endif + } + + /* Normal Logging */ + void normal_log(const char* category, const char* format, ...) { +#if defined(PRIVATE_LOG_PATH) || defined(PUBLIC_LOGGING) + va_list ap; + va_start(ap, format); + internal_log(category,format,ap); + va_end(ap); +#endif + } + +protected: + /* Members */ + string instanceName; + fmi2Type fmuType; + string fmuGUID; + string fmuResourceLocation; + bool visible; + bool loggingOn; + set loggingCategories; + fmi2CallbackFunctions functions; + fmi2Boolean boolean_vars[FMI_BOOLEAN_VARS]; + fmi2Integer integer_vars[FMI_INTEGER_VARS]; + fmi2Real real_vars[FMI_REAL_VARS]; + string string_vars[FMI_STRING_VARS]; + //string* currentBuffer; + string currentBuffer; + //string* lastBuffer; + string lastBuffer; + + /* Simple Accessors */ + fmi2Boolean fmi_valid() { return boolean_vars[FMI_BOOLEAN_VALID_IDX]; } + void set_fmi_valid(fmi2Boolean value) { boolean_vars[FMI_BOOLEAN_VALID_IDX]=value; } + fmi2Integer fmi_count() { return integer_vars[FMI_INTEGER_COUNT_IDX]; } + void set_fmi_count(fmi2Integer value) { integer_vars[FMI_INTEGER_COUNT_IDX]=value; } + + /* Protocol Buffer Accessors */ + //void set_fmi_sensor_view_out(const osi3::SensorView& data); + void set_fmi_sensor_view_out(); + void reset_fmi_sensor_view_out(); + +}; diff --git a/examples/OSMPDummySource_flat/OSMPDummySourceConfig.in.h b/examples/OSMPDummySource_flat/OSMPDummySourceConfig.in.h new file mode 100644 index 0000000..868a5fd --- /dev/null +++ b/examples/OSMPDummySource_flat/OSMPDummySourceConfig.in.h @@ -0,0 +1,15 @@ +/* + * PMSF FMU Framework for FMI 2.0 Co-Simulation FMUs + * + * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#cmakedefine PUBLIC_LOGGING +#cmakedefine PRIVATE_LOG_PATH "@PRIVATE_LOG_PATH@" +#cmakedefine VERBOSE_FMI_LOGGING +#cmakedefine DEBUG_BREAKS +#define FMU_GUID "@FMUGUID@" diff --git a/examples/OSMPDummySource_flat/modelDescription.in.xml b/examples/OSMPDummySource_flat/modelDescription.in.xml new file mode 100644 index 0000000..a7502ce --- /dev/null +++ b/examples/OSMPDummySource_flat/modelDescription.in.xml @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 33c99a5d41dde089a024a767f8fd26cf0fbcd029 Mon Sep 17 00:00:00 2001 From: linnhoff Date: Fri, 4 Feb 2022 19:32:32 +0100 Subject: [PATCH 03/13] reimplement buffer swap Signed-off-by: linnhoff --- examples/OSMPDummySensor_flat/OSMPDummySensor.cpp | 2 +- examples/OSMPDummySource_flat/OSMPDummySource.cpp | 5 +---- examples/OSMPDummySource_flat/OSMPDummySource.h | 2 -- 3 files changed, 2 insertions(+), 7 deletions(-) diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index 105f920..04d7eaf 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -170,7 +170,7 @@ void COSMPDummySensor::set_fmi_sensor_data_out() integer_vars[FMI_INTEGER_SENSORDATA_OUT_SIZE_IDX]=(fmi2Integer)currentOutputBuffer.length(); normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORDATA_OUT_BASELO_IDX],currentOutputBuffer.data()); - //swap(currentOutputBuffer,lastOutputBuffer); + swap(currentOutputBuffer,lastOutputBuffer); } void COSMPDummySensor::reset_fmi_sensor_data_out() diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index b1c16d4..d23bfb4 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -112,16 +112,13 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) #endif } -//void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) void COSMPDummySource::set_fmi_sensor_view_out() { - //data.SerializeToString(currentBuffer); - //encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); encode_pointer_to_integer(currentBuffer.data(), integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX], integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer.length(); normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer.data()); - //swap(currentBuffer,lastBuffer); + swap(currentBuffer,lastBuffer); } void COSMPDummySource::reset_fmi_sensor_view_out() diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.h b/examples/OSMPDummySource_flat/OSMPDummySource.h index 4f987b9..91b05d7 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.h +++ b/examples/OSMPDummySource_flat/OSMPDummySource.h @@ -192,9 +192,7 @@ class COSMPDummySource { fmi2Integer integer_vars[FMI_INTEGER_VARS]; fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; - //string* currentBuffer; string currentBuffer; - //string* lastBuffer; string lastBuffer; /* Simple Accessors */ From 514f537f3e3804201435999988146247f4f1d564 Mon Sep 17 00:00:00 2001 From: linnhoff Date: Fri, 18 Feb 2022 10:31:47 +0100 Subject: [PATCH 04/13] added exemplary reflections and performance logging for flatbuffers Signed-off-by: linnhoff --- .../OSMPDummySensor_flat/OSMPDummySensor.cpp | 112 +++++++++++++++++- .../OSMPDummySource_flat/OSMPDummySource.cpp | 108 ++++++++++++++++- 2 files changed, 214 insertions(+), 6 deletions(-) diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index 04d7eaf..703d3ce 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -111,6 +111,7 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) bool COSMPDummySensor::get_fmi_sensor_view_config(osi3::SensorViewConfiguration& data) { + //todo: sensor view config currently not implemented /*if (integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_BASELO_IDX]); 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& } else { return false; }*/ - return false; //todo + return false; } void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data) { + //todo: sensor view config currently not implemented /*data.SerializeToString(currentConfigRequestBuffer); encode_pointer_to_integer(currentConfigRequestBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX]); integer_vars[FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX]=(fmi2Integer)currentConfigRequestBuffer->length(); 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()); swap(currentConfigRequestBuffer,lastConfigRequestBuffer);*/ - //todo } void COSMPDummySensor::reset_fmi_sensor_view_config_request() @@ -182,6 +183,7 @@ void COSMPDummySensor::reset_fmi_sensor_data_out() void COSMPDummySensor::refresh_fmi_sensor_view_config_request() { + //todo: sensor view config currently not implemented /*osi3::SensorViewConfiguration config; if (get_fmi_sensor_view_config(config)) set_fmi_sensor_view_config_request(config); @@ -199,7 +201,6 @@ void COSMPDummySensor::refresh_fmi_sensor_view_config_request() generic->set_field_of_view_vertical(3.14); set_fmi_sensor_view_config_request(config); }*/ - //todo } /* @@ -285,9 +286,54 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real DEBUGBREAK(); flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; + std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in(); + std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + if (sensor_view_in) { + //// Lidar Detections + std::vector> lidar_detection_vector; + if (sensor_view_in->lidar_sensor_view()) { + int no_of_layers = 32; // the number of layers of every lidar front-end + double azimuth_fov = 360.0; // Azimuth angle FoV in ° + int rays_per_beam_vertical = 3; // vertical super-sampling factor + int rays_per_beam_horizontal = 6; // horizontal super-sampling factor + 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 + double beam_step_elevation = 0.3; // simplified equidistant beam spacing + double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB + int rays_per_layer = azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8; + double const_distance = 10.0; + double speed_of_light = 299792458.0; + size_t num_reflections = sensor_view_in->lidar_sensor_view()->Get(0)->reflection()->size(); + int layer_idx = -1; + for (size_t reflection_idx = 0; reflection_idx < num_reflections; reflection_idx++) { + if ((reflection_idx % rays_per_layer) == 0) layer_idx++; + auto current_reflection = sensor_view_in->lidar_sensor_view()->Get(0)->reflection()->Get(reflection_idx); + if (reflection_idx % 18 == 0) { //18 times super-sampling + //todo: generate lidar detection + double distance = current_reflection->time_of_flight() * speed_of_light / 2; + double azimuth_deg = double(reflection_idx % rays_per_layer) * beam_step_azimuth; + double elevation_deg = layer_idx * beam_step_elevation - 5; //start at -5° for simplification + auto detection_position = osi3::CreateSpherical3d(builder, distance, azimuth_deg*M_PI/180, elevation_deg*M_PI/180); + osi3::LidarDetectionBuilder lidar_detection_builder(builder); + lidar_detection_builder.add_position(detection_position); + lidar_detection_vector.push_back(lidar_detection_builder.Finish()); + } + } + } + auto lidar_detection_vector_flatvector = builder.CreateVector(lidar_detection_vector); + auto lidar_sensor_builder = osi3::LidarDetectionDataBuilder(builder); + lidar_sensor_builder.add_detection(lidar_detection_vector_flatvector); + auto lidar_sensor = lidar_sensor_builder.Finish(); + std::vector> lidar_sensor_vector; + lidar_sensor_vector.push_back(lidar_sensor); + auto lidar_sensor_vector_flatvector = builder.CreateVector(lidar_sensor_vector); + auto feature_data_builder = osi3::FeatureDataBuilder(builder); + feature_data_builder.add_lidar_sensor(lidar_sensor_vector_flatvector); + auto feature_data = feature_data_builder.Finish(); + + //// Moving Objects double ego_x=0, ego_y=0, ego_z=0; const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id(); normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id->value()); @@ -383,8 +429,13 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real sensor_data_builder.add_timestamp(timestamp); //sensor_data_builder.add_version(interface_version); sensor_data_builder.add_moving_object(detected_moving_object_flatvector); + if (sensor_view_in->lidar_sensor_view()) { + sensor_data_builder.add_feature_data(feature_data); + } auto sensor_data = sensor_data_builder.Finish(); + std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + builder.Finish(sensor_data); auto uint8_buffer = builder.GetBufferPointer(); auto size = builder.GetSize(); @@ -397,6 +448,61 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real set_fmi_valid(true); set_fmi_count(moving_obj_counter); + std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + + //// Performance logging + std::ifstream f(fileName.c_str()); + bool fileExists = f.is_open(); + f.close(); + + std::ofstream logFile; + if(!fileExists) { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream time_string; + time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json"); + fileName += time_string.str(); + logFile.open (fileName, std::ios_base::app); + logFile << "{" << std::endl; + logFile << "\t\"Header\": {" << std::endl; + logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl; + logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl; + logFile << "\t\t\"EventTypes\": [\"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl; + logFile << "\t\t\"FormatVersion\": {" << std::endl; + logFile << "\t\t\t\"Major\": 1," << std::endl; + logFile << "\t\t\t\"Minor\": 0," << std::endl; + logFile << "\t\t\t\"Patch\": 0," << std::endl; + logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t}," << std::endl; + logFile << "\t\"Data\": [" << std::endl; + logFile << "\t\t{" << std::endl; + logFile << "\t\t\t\"Instance\": {" << std::endl; + logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Flatbuf\"" << std::endl; + /*logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Flatbuf\"" << "," << std::endl; + logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl; + logFile << "\t\t\t\t\t\"version_major\": " << sensor_view_in->version()->version_major() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_minor\": " << sensor_view_in->version()->version_minor() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_patch\": " << sensor_view_in->version()->version_patch() << std::endl;* + logFile << "\t\t\t\t}" << std::endl;*/ + logFile << "\t\t\t}," << std::endl; + logFile << "\t\t\t\"OsiEvents\": [" << std::endl; + } else { + logFile.open (fileName, std::ios_base::app); + } + + if(fileExists) { + logFile << "," << std::endl; + } + size_t sensorDataSize = builder.GetSize(); + double osiSimTime = (double)sensor_view_in->global_ground_truth()->timestamp()->seconds() + (double)sensor_view_in->global_ground_truth()->timestamp()->nanos() * 0.000000001; + + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; + logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; + logFile.close(); + } else { /* We have no valid input, so no valid output */ normal_log("OSI","No valid input, therefore providing no valid output."); diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index d23bfb4..a38c6a4 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -198,12 +198,60 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); + std::chrono::milliseconds start_source_calc = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating SensorView at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); /* We act as GroundTruth Source */ + + //// Lidar Reflections + auto mounting_pos_position = osi3::CreateVector3d(builder, 1.5205, 0.0, 1.232); + osi3::MountingPositionBuilder mounting_position_builder(builder); + mounting_position_builder.add_position(mounting_pos_position); + auto mounting_position = mounting_position_builder.Finish(); + + osi3::LidarSensorViewConfigurationBuilder lidar_sensor_view_configuration_builder(builder); + lidar_sensor_view_configuration_builder.add_field_of_view_horizontal(145.0 / 180 * M_PI); + lidar_sensor_view_configuration_builder.add_field_of_view_vertical(3.2 / 180 * M_PI); + lidar_sensor_view_configuration_builder.add_mounting_position(mounting_position); + auto lidar_sensor_view_configuration = lidar_sensor_view_configuration_builder.Finish(); + + int no_of_layers = 32; // the number of layers of every lidar front-end + double azimuth_fov = 360.0; // Azimuth angle FoV in ° + int rays_per_beam_vertical = 3; // vertical super-sampling factor + int rays_per_beam_horizontal = 6; // horizontal super-sampling factor + 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 + double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB + int rays_per_beam = rays_per_beam_vertical * rays_per_beam_horizontal; + double const_distance = 10.0; + double speed_of_light = 299792458.0; + + std::vector> reflection_vector; + // Simulation of lower number of rays because of SET Level improvements + for (int elevation_idx = 0; elevation_idx < no_of_layers * rays_per_beam_vertical; elevation_idx++) { + for (int azimuth_idx = 0; azimuth_idx < azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8; azimuth_idx++) { //assume 80% of rays generate reflection + double attenuation = 1.0; + osi3::LidarSensorView_::ReflectionBuilder reflection_builder(builder); + reflection_builder.add_time_of_flight(const_distance * 2.0 / speed_of_light); + reflection_builder.add_signal_strength(max_emitted_signal_strength_in_dB + 10 * std::log10(attenuation) - 10 * std::log10(rays_per_beam)); // assuming equal distribution of beam power per ray + auto ray = reflection_builder.Finish(); + reflection_vector.push_back(ray); + } + } + + auto reflection_flat_vector = builder.CreateVector(reflection_vector); + osi3::LidarSensorViewBuilder lidar_sensor_view_builder(builder); + lidar_sensor_view_builder.add_view_configuration(lidar_sensor_view_configuration); + lidar_sensor_view_builder.add_reflection(reflection_flat_vector); + auto lidar_sensor_view = lidar_sensor_view_builder.Finish(); + std::vector> lidar_sensor_view_vector; + lidar_sensor_view_vector.push_back(lidar_sensor_view); + auto lidar_sensor_view_flatvector = builder.CreateVector(lidar_sensor_view_vector); + + //// Moving Objects static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; static double source_x_offsets[10] = { 0.0, 40.0, 100.0, 100.0, 0.0, 150.0, 5.0, 45.0, 85.0, 125.0 }; static double source_x_speeds[10] = { 29.0, 30.0, 31.0, 25.0, 26.0, 28.0, 20.0, 22.0, 22.5, 23.0 }; @@ -220,7 +268,6 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real osi3::MovingObject_::VehicleClassification_::Type::TYPE_BUS }; std::vector> moving_object_vector; - // Vehicles for (unsigned int i=0;i<10;i++) { osi3::IdentifierBuilder id_builder(builder); id_builder.add_value(10+i); @@ -282,8 +329,11 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real sensor_view_builder.add_host_vehicle_id(host_vehicle_id); sensor_view_builder.add_timestamp(timestamp); sensor_view_builder.add_global_ground_truth(ground_truth); + sensor_view_builder.add_lidar_sensor_view(lidar_sensor_view_flatvector); auto sensor_view = sensor_view_builder.Finish(); + std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + builder.Finish(sensor_view); auto uint8_buffer = builder.GetBufferPointer(); auto size = builder.GetSize(); @@ -294,6 +344,60 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real set_fmi_valid(true); set_fmi_count((int)moving_object_vector.size()); + std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + + //// Performance logging + auto sensor_view_in = flatbuffers::GetRoot(uint8_buffer); + std::ifstream f(fileName.c_str()); + bool fileExists = f.is_open(); + f.close(); + + std::ofstream logFile; + if(!fileExists) { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream time_string; + time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json"); + fileName += time_string.str(); + logFile.open (fileName, std::ios_base::app); + logFile << "{" << std::endl; + logFile << "\t\"Header\": {" << std::endl; + logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl; + logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl; + logFile << "\t\t\"EventTypes\": [\"StartSourceCalc\", \"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl; + logFile << "\t\t\"FormatVersion\": {" << std::endl; + logFile << "\t\t\t\"Major\": 1," << std::endl; + logFile << "\t\t\t\"Minor\": 0," << std::endl; + logFile << "\t\t\t\"Patch\": 0," << std::endl; + logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t}," << std::endl; + logFile << "\t\"Data\": [" << std::endl; + logFile << "\t\t{" << std::endl; + logFile << "\t\t\t\"Instance\": {" << std::endl; + logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySource Flatbuf\"" << std::endl; + /*logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySource Flatbuf\"" << "," << std::endl; + logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl; + logFile << "\t\t\t\t\t\"version_major\": " << sensor_view_in->version()->version_major() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_minor\": " << sensor_view_in->version()->version_minor() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_patch\": " << sensor_view_in->version()->version_patch() << std::endl; + logFile << "\t\t\t\t}" << std::endl;*/ + logFile << "\t\t\t}," << std::endl; + logFile << "\t\t\t\"OsiEvents\": [" << std::endl; + } else { + logFile.open (fileName, std::ios_base::app); + } + + if(fileExists) { + logFile << "," << std::endl; + } + size_t sensorViewSize = builder.GetSize(); + double osiSimTime = (double)sensor_view_in->global_ground_truth()->timestamp()->seconds() + (double)sensor_view_in->global_ground_truth()->timestamp()->nanos() * 0.000000001; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)start_source_calc.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; + logFile.close(); + return fmi2OK; } @@ -322,9 +426,7 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy loggingOn(!!theloggingOn) { //currentBuffer = new string(); - //currentBuffer = new uint8_t(); //lastBuffer = new string(); - //lastBuffer = new uint8_t(); loggingCategories.clear(); loggingCategories.insert("FMI"); loggingCategories.insert("OSMP"); From 490d98489390ac4b4b25654c2d591e6e3a220d2a Mon Sep 17 00:00:00 2001 From: linnhoff Date: Fri, 18 Feb 2022 11:09:03 +0100 Subject: [PATCH 05/13] added reflections and performance logging to protobuf examples Signed-off-by: linnhoff --- examples/OSMPDummySensor/OSMPDummySensor.cpp | 108 ++++++++++++++ examples/OSMPDummySource/OSMPDummySource.cpp | 142 ++++++++++++++++--- 2 files changed, 234 insertions(+), 16 deletions(-) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index fe5f62b..0b33eb0 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -48,6 +48,10 @@ #include #include #include +#include +#include +#include +#include //included for windows compatibility using namespace std; @@ -55,6 +59,12 @@ using namespace std; ofstream COSMPDummySensor::private_log_file; #endif +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySensor_Protobuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySensor_Protobuf_timing"; +#endif + /* * ProtocolBuffer Accessors */ @@ -133,6 +143,7 @@ bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data) if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) { void* buffer = decode_integer_to_pointer(integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_IN_BASELO_IDX]); 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); + 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); data.ParseFromArray(buffer,integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX]); return true; } else { @@ -263,7 +274,41 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real osi3::SensorData currentOut; double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); + std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); if (get_fmi_sensor_view_in(currentIn)) { + std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + //// Lidar Detections + if (currentIn.lidar_sensor_view_size() > 0) { + auto lidar_sensor = currentOut.mutable_feature_data()->add_lidar_sensor(); + int no_of_layers = 32; // the number of layers of every lidar front-end + double azimuth_fov = 360.0; // Azimuth angle FoV in ° + int rays_per_beam_vertical = 3; // vertical super-sampling factor + int rays_per_beam_horizontal = 6; // horizontal super-sampling factor + 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 + double beam_step_elevation = 0.3; // simplified equidistant beam spacing + double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB + int rays_per_layer = azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8; + double const_distance = 10.0; + double speed_of_light = 299792458.0; + size_t num_reflections = currentIn.lidar_sensor_view(0).reflection_size(); + int layer_idx = -1; + for (int reflection_idx = 0; reflection_idx < num_reflections; reflection_idx++) { + if ((reflection_idx % rays_per_layer) == 0) layer_idx++; + auto current_reflection = currentIn.lidar_sensor_view(0).reflection(reflection_idx); + if (reflection_idx % 18 == 0) { //18 times super-sampling + //todo: generate lidar detection + double distance = current_reflection.time_of_flight() * speed_of_light / 2; + double azimuth_deg = double(reflection_idx % rays_per_layer) * beam_step_azimuth; + double elevation_deg = layer_idx * beam_step_elevation - 5; //start at -5° for simplification + auto current_detection = lidar_sensor->add_detection(); + current_detection->mutable_position()->set_distance(distance); + current_detection->mutable_position()->set_azimuth(azimuth_deg*M_PI/180); + current_detection->mutable_position()->set_elevation(elevation_deg*M_PI/180); + } + } + } + + //// Moving Objects double ego_x=0, ego_y=0, ego_z=0; osi3::Identifier ego_id = currentIn.global_ground_truth().host_vehicle_id(); normal_log("OSI","Looking for EgoVehicle with ID: %llu",ego_id.value()); @@ -333,10 +378,66 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real } }); normal_log("OSI","Mapped %d vehicles to output", i); + /* Serialize */ + std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); set_fmi_sensor_data_out(currentOut); set_fmi_valid(true); set_fmi_count(currentOut.moving_object_size()); + std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + + //Performance logging + std::ifstream f(fileName.c_str()); + bool fileExists = f.is_open(); + f.close(); + + std::ofstream logFile; + if(!fileExists) { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream time_string; + time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json"); + fileName += time_string.str(); + logFile.open (fileName, std::ios_base::app); + logFile << "{" << std::endl; + logFile << "\t\"Header\": {" << std::endl; + logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl; + logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl; + logFile << "\t\t\"EventTypes\": [\"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl; + logFile << "\t\t\"FormatVersion\": {" << std::endl; + logFile << "\t\t\t\"Major\": 1," << std::endl; + logFile << "\t\t\t\"Minor\": 0," << std::endl; + logFile << "\t\t\t\"Patch\": 0," << std::endl; + logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t}," << std::endl; + logFile << "\t\"Data\": [" << std::endl; + logFile << "\t\t{" << std::endl; + logFile << "\t\t\t\"Instance\": {" << std::endl; + logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Protobuf\"" << std::endl; + /*logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySensor Protobuf\"" << "," << std::endl; + logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl; + logFile << "\t\t\t\t\t\"version_major\": " << sensor_view_in->version()->version_major() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_minor\": " << sensor_view_in->version()->version_minor() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_patch\": " << sensor_view_in->version()->version_patch() << std::endl;* + logFile << "\t\t\t\t}" << std::endl;*/ + logFile << "\t\t\t}," << std::endl; + logFile << "\t\t\t\"OsiEvents\": [" << std::endl; + } else { + logFile.open (fileName, std::ios_base::app); + } + + if(fileExists) { + logFile << "," << std::endl; + } + size_t sensorDataSize = currentOut.ByteSize(); + double osiSimTime = (double)currentOut.timestamp().seconds() + (float)currentOut.timestamp().nanos() * 0.000000001; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; //todo: sensor view size + logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; + logFile.close(); + } else { /* We have no valid input, so no valid output */ normal_log("OSI","No valid input, therefore providing no valid output."); @@ -661,6 +762,13 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySensor* myc = (COSMPDummySensor*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); return myc->Terminate(); } diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index b0e17e0..cfad1bf 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -44,10 +44,14 @@ #endif #include -#include +//#include #include #include #include +#include +#include +#include +#include //included for windows compatibility using namespace std; @@ -55,6 +59,12 @@ using namespace std; ofstream COSMPDummySource::private_log_file; #endif +#ifdef _WIN32 +std::string fileName = "C:/tmp/OSMPDummySource_Protobuf_timing"; +#else +std::string fileName = "/tmp/OSMPDummySource_Protobuf_timing"; +#endif + /* * ProtocolBuffer Accessors */ @@ -106,6 +116,7 @@ void COSMPDummySource::set_fmi_sensor_view_out(const osi3::SensorView& data) encode_pointer_to_integer(currentBuffer->data(),integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX]); integer_vars[FMI_INTEGER_SENSORVIEW_OUT_SIZE_IDX]=(fmi2Integer)currentBuffer->length(); normal_log("OSMP","Providing %08X %08X, writing from %p ...",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer->data()); + std::printf("Providing %08X %08X, writing from %p ...\n",integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASEHI_IDX],integer_vars[FMI_INTEGER_SENSORVIEW_OUT_BASELO_IDX],currentBuffer->data()); swap(currentBuffer,lastBuffer); } @@ -186,6 +197,7 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); + std::chrono::milliseconds start_source_calc = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); osi3::SensorView currentOut; double time = currentCommunicationPoint+communicationStepSize; @@ -193,20 +205,6 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real normal_log("OSI","Calculating SensorView at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); /* We act as GroundTruth Source */ - static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; - static double source_x_offsets[10] = { 0.0, 40.0, 100.0, 100.0, 0.0, 150.0, 5.0, 45.0, 85.0, 125.0 }; - static double source_x_speeds[10] = { 29.0, 30.0, 31.0, 25.0, 26.0, 28.0, 20.0, 22.0, 22.5, 23.0 }; - static osi3::MovingObject_VehicleClassification_Type source_veh_types[10] = { - osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN, - osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, - osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE, - osi3::MovingObject_VehicleClassification_Type_TYPE_BUS }; currentOut.Clear(); currentOut.mutable_version()->CopyFrom(osi3::InterfaceVersion::descriptor()->file()->options().GetExtension(osi3::current_interface_version)); @@ -219,7 +217,51 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real currentGT->mutable_timestamp()->set_nanos((int)((time - floor(time))*1000000000.0)); currentGT->mutable_host_vehicle_id()->set_value(14); - // Vehicles + //// Lidar Reflections + auto lidar_sensor_view = currentOut.add_lidar_sensor_view(); + auto lidar_sensor_view_configuration = lidar_sensor_view->mutable_view_configuration(); + lidar_sensor_view_configuration->set_field_of_view_horizontal(145.0 / 180 * M_PI); + lidar_sensor_view_configuration->set_field_of_view_vertical(3.2 / 180 * M_PI); + lidar_sensor_view_configuration->mutable_mounting_position()->mutable_position()->set_x(1.5205); // from middle of rear axle, in m // 1.3705 + 0.15 + lidar_sensor_view_configuration->mutable_mounting_position()->mutable_position()->set_y(0.0); // from middle of rear axle, in m + lidar_sensor_view_configuration->mutable_mounting_position()->mutable_position()->set_z(1.232); // from middle of rear axle, in m // 0.382 + 0.85 + + int no_of_layers = 32; // the number of layers of every lidar front-end + double azimuth_fov = 360.0; // Azimuth angle FoV in ° + int rays_per_beam_vertical = 3; // vertical super-sampling factor + int rays_per_beam_horizontal = 6; // horizontal super-sampling factor + 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 + double max_emitted_signal_strength_in_dB = 10 * std::log10(0.5); // maximal emitted signal strength in dB + int rays_per_beam = rays_per_beam_vertical * rays_per_beam_horizontal; + double const_distance = 10.0; + double speed_of_light = 299792458.0; + + // Simulation of lower number of rays because of SET Level improvements + for (int elevation_idx = 0; elevation_idx < no_of_layers * rays_per_beam_vertical; elevation_idx++) { + for (int azimuth_idx = 0; azimuth_idx < azimuth_fov/beam_step_azimuth*rays_per_beam_horizontal*0.8; azimuth_idx++) { //assume 80% of rays generate reflection + double attenuation = 1.0; + auto ray = lidar_sensor_view->add_reflection(); + ray->set_time_of_flight(const_distance * 2.0 / speed_of_light); + ray->set_signal_strength(max_emitted_signal_strength_in_dB + 10 * std::log10(attenuation) - 10 * std::log10(rays_per_beam)); // assuming equal distribution of beam power per ray + } + } + + //// Moving Objects + static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; + static double source_x_offsets[10] = { 0.0, 40.0, 100.0, 100.0, 0.0, 150.0, 5.0, 45.0, 85.0, 125.0 }; + static double source_x_speeds[10] = { 29.0, 30.0, 31.0, 25.0, 26.0, 28.0, 20.0, 22.0, 22.5, 23.0 }; + static osi3::MovingObject_VehicleClassification_Type source_veh_types[10] = { + osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_DELIVERY_VAN, + osi3::MovingObject_VehicleClassification_Type_TYPE_LUXURY_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_MEDIUM_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_COMPACT_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_SMALL_CAR, + osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE, + osi3::MovingObject_VehicleClassification_Type_TYPE_BUS }; + for (unsigned int i=0;i<10;i++) { osi3::MovingObject *veh = currentGT->add_moving_object(); veh->mutable_id()->set_value(10+i); @@ -229,6 +271,9 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real auto vehlights = vehclass->mutable_light_state(); vehlights->set_indicator_state(osi3::MovingObject_VehicleClassification_LightState_IndicatorState_INDICATOR_STATE_OFF); vehlights->set_brake_light_state(osi3::MovingObject_VehicleClassification_LightState_BrakeLightState_BRAKE_LIGHT_STATE_OFF); + veh->mutable_vehicle_attributes()->mutable_bbcenter_to_rear()->set_x(1); + veh->mutable_vehicle_attributes()->mutable_bbcenter_to_rear()->set_y(0); + veh->mutable_vehicle_attributes()->mutable_bbcenter_to_rear()->set_x(-0.3); veh->mutable_base()->mutable_dimension()->set_height(1.5); veh->mutable_base()->mutable_dimension()->set_width(2.0); veh->mutable_base()->mutable_dimension()->set_length(5.0); @@ -250,9 +295,66 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real normal_log("OSI","GT: Adding Vehicle %d[%llu] Absolute Position: %f,%f,%f Velocity (%f,%f,%f)",i,veh->id().value(),veh->base().position().x(),veh->base().position().y(),veh->base().position().z(),veh->base().velocity().x(),veh->base().velocity().y(),veh->base().velocity().z()); } + std::cout << "DummySource time doCalc(): " << time << std::endl; + + //check if file exists + std::ifstream f(fileName.c_str()); + bool fileExists = f.is_open(); + f.close(); + + std::ofstream logFile; + if(!fileExists) { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream time_string; + time_string << std::put_time(std::localtime(&in_time_t), "_%H-%M-%S.json"); + fileName += time_string.str(); + logFile.open (fileName, std::ios_base::app); + logFile << "{" << std::endl; + logFile << "\t\"Header\": {" << std::endl; + logFile << "\t\t\"OsiMessages\": [\"osi3::SensorView\", \"osi3::SensorData\"]," << std::endl; + logFile << "\t\t\"EventFields\": [\"EventId\", \"GlobalTime\", \"SimulationTime\", \"MessageId\", \"SizeValueReference\", \"MessageSize\"]," << std::endl; + logFile << "\t\t\"EventTypes\": [\"StartSourceCalc\", \"StartOSISerialize\", \"StopOSISerialize\", \"StartOSIDeserialize\", \"StopOSIDeserialize\"]," << std::endl; + logFile << "\t\t\"FormatVersion\": {" << std::endl; + logFile << "\t\t\t\"Major\": 1," << std::endl; + logFile << "\t\t\t\"Minor\": 0," << std::endl; + logFile << "\t\t\t\"Patch\": 0," << std::endl; + logFile << "\t\t\t\"PreRelease\": \"beta\"" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t}," << std::endl; + logFile << "\t\"Data\": [" << std::endl; + logFile << "\t\t{" << std::endl; + logFile << "\t\t\t\"Instance\": {" << std::endl; + logFile << "\t\t\t\t\"ModelIdentity\": " << "\"OSMPDummySource Protobuf\"" << "," << std::endl; + logFile << "\t\t\t\t\"OsiVersion\": {" << std::endl; + logFile << "\t\t\t\t\t\"version_major\": "<< currentOut.version().version_major() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_minor\": "<< currentOut.version().version_minor() << "," << std::endl; + logFile << "\t\t\t\t\t\"version_patch\": "<< currentOut.version().version_patch() << std::endl; + logFile << "\t\t\t\t}" << std::endl; + logFile << "\t\t\t}," << std::endl; + logFile << "\t\t\t\"OsiEvents\": [" << std::endl; + } else { + logFile.open (fileName, std::ios_base::app); + } + + float osiSimTime = currentOut.global_ground_truth().timestamp().seconds() + (float)currentOut.global_ground_truth().timestamp().nanos() * 0.000000001; + + std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + set_fmi_sensor_view_out(currentOut); set_fmi_valid(true); set_fmi_count(currentGT->moving_object_size()); + std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + + if(fileExists) { + logFile << "," << std::endl; + } + int sensorViewSize = currentOut.ByteSize(); + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)start_source_calc.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; + logFile.close(); + return fmi2OK; } @@ -557,6 +659,14 @@ extern "C" { FMI2_Export fmi2Status fmi2Terminate(fmi2Component c) { COSMPDummySource* myc = (COSMPDummySource*)c; + std::ofstream logFile; + logFile.open(fileName, std::ios_base::app); + logFile << std::endl << "\t\t\t]" << std::endl; + logFile << "\t\t}" << std::endl; + logFile << "\t]" << std::endl; + logFile << "}" << std::endl; + logFile.close(); + return myc->Terminate(); } From 22f233790bc2c4779f3758ecca3587b7f8db209d Mon Sep 17 00:00:00 2001 From: PhRosenberger Date: Fri, 18 Feb 2022 15:40:54 +0100 Subject: [PATCH 06/13] Add Persival GmbH credits where necessary Signed-off-by: PhRosenberger --- examples/OSMPDummySensor_flat/OSMPDummySensor.cpp | 2 ++ examples/OSMPDummySensor_flat/OSMPDummySensor.h | 2 ++ examples/OSMPDummySource_flat/OSMPDummySource.cpp | 2 ++ examples/OSMPDummySource_flat/OSMPDummySource.h | 2 ++ 4 files changed, 8 insertions(+) diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index 703d3ce..985befa 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -3,6 +3,8 @@ * * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai * + * (C) 2022 Persival GmbH Clemens Linnhoff + * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.h b/examples/OSMPDummySensor_flat/OSMPDummySensor.h index 78b730c..4fbef1e 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.h +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.h @@ -3,6 +3,8 @@ * * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai * + * (C) 2022 Persival GmbH Clemens Linnhoff + * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index a38c6a4..4eb1cd5 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -3,6 +3,8 @@ * * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai * + * (C) 2022 Persival GmbH Clemens Linnhoff + * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.h b/examples/OSMPDummySource_flat/OSMPDummySource.h index 91b05d7..de4dbc6 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.h +++ b/examples/OSMPDummySource_flat/OSMPDummySource.h @@ -3,6 +3,8 @@ * * (C) 2016 -- 2018 PMSF IT Consulting Pierre R. Mai * + * (C) 2022 Persival GmbH Clemens Linnhoff + * * This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. From f936b6838a3668c9cfa945cda682eb4216b4f9cb Mon Sep 17 00:00:00 2001 From: PhRosenberger Date: Fri, 18 Feb 2022 15:38:54 +0100 Subject: [PATCH 07/13] Fix includes for M_PI on Windows Signed-off-by: PhRosenberger --- examples/OSMPDummySensor_flat/OSMPDummySensor.cpp | 11 ++++++++++- examples/OSMPDummySource_flat/OSMPDummySource.cpp | 11 ++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index 985befa..4d10c17 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -10,6 +10,10 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + #include "OSMPDummySensor.h" /* @@ -49,9 +53,14 @@ #include #include #include -#include #include +#ifdef _WIN32 +#include +#else +#include +#endif + using namespace std; #ifdef PRIVATE_LOG_PATH diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index 4eb1cd5..fe70bb5 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -10,6 +10,10 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif + #include "OSMPDummySource.h" /* @@ -49,12 +53,17 @@ //#include #include #include -#include #include #include #include #include //included for windows compatibility +#ifdef _WIN32 +#include +#else +#include +#endif + using namespace std; #ifdef PRIVATE_LOG_PATH From ac11a7d3f58e4833a884598290bbdd7a8ebdb372 Mon Sep 17 00:00:00 2001 From: Martin Kirchengast Date: Wed, 11 May 2022 14:06:14 +0200 Subject: [PATCH 08/13] Shared library OSI3 linking --- .gitignore | 2 +- examples/OSMPDummySensor/CMakeLists.txt | 2 +- examples/OSMPDummySource/CMakeLists.txt | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitignore b/.gitignore index cb5ae72..9ba539f 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1 @@ -examples/build +examples/build* diff --git a/examples/OSMPDummySensor/CMakeLists.txt b/examples/OSMPDummySensor/CMakeLists.txt index 625725d..b926610 100644 --- a/examples/OSMPDummySensor/CMakeLists.txt +++ b/examples/OSMPDummySensor/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySensor) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) diff --git a/examples/OSMPDummySource/CMakeLists.txt b/examples/OSMPDummySource/CMakeLists.txt index e7c2f48..bbef886 100644 --- a/examples/OSMPDummySource/CMakeLists.txt +++ b/examples/OSMPDummySource/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySource) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) From 42d1ade1cc3a3b43f39ad42a85c07a9881b07120 Mon Sep 17 00:00:00 2001 From: Martin Kirchengast Date: Tue, 17 May 2022 16:34:48 +0200 Subject: [PATCH 09/13] Timing analysis script --- scripts/timing_analyze.py | 60 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 scripts/timing_analyze.py diff --git a/scripts/timing_analyze.py b/scripts/timing_analyze.py new file mode 100644 index 0000000..d50b04f --- /dev/null +++ b/scripts/timing_analyze.py @@ -0,0 +1,60 @@ +import json +import numpy as np + +#timing_json_path = '/tmp/OSMPDummySensor_flatbuf_timing_13-30-52.json' +#timing_json_path = '/tmp/OSMPDummySource_flatbuf_timing_13-30-52.json' +#timing_json_path = '/tmp/OSMPDummySensor_flatbuf_timing_15-35-40.json' +timing_json_path = '/tmp/OSMPDummySource_flatbuf_timing_15-35-40.json' + +#timing_json_path = '/tmp/OSMPDummySensor_Protobuf_timing_13-23-09.json' +#timing_json_path = '/tmp/OSMPDummySource_Protobuf_timing_13-23-09.json' +#timing_json_path = '/tmp/OSMPDummySensor_Protobuf_timing_15-29-42.json' +#timing_json_path = '/tmp/OSMPDummySource_Protobuf_timing_15-29-41.json' + +# Opening JSON file +f = open(timing_json_path) + +# returns JSON object as +# a dictionary +data = json.load(f) + +if 'OSMPDummySensor' in data['Data'][0]['Instance']['ModelIdentity']: + startEvent = 2 + nextEvent = 2 + eventCount = int(4) + timeStrings = ['Model deserialize', 'Model calculation', 'Model serialize', 'Total'] +else: + startEvent = 0 + nextEvent = 0 + eventCount = int(3) + timeStrings = ['Source generate', 'Source serialize', 'Total'] + +numTimeSteps = len(data['Data'][0]['OsiEvents']) // eventCount +timingMat = np.zeros((numTimeSteps, eventCount - 1)) +matIdx = 0 +prevTimeStamp = 0 + +# Iterating through the json +# list +for event in data['Data'][0]['OsiEvents']: + if event[0] != nextEvent: + print('Error in log event sequence') + break + curTimeStamp = event[1] + if event[0] != startEvent: + timingMat[np.unravel_index(matIdx, timingMat.shape)] = curTimeStamp - prevTimeStamp + matIdx += 1 + + nextEvent += 1 + nextEvent = nextEvent % eventCount + prevTimeStamp = curTimeStamp + +timingAvg = timingMat.sum(axis = 0) / numTimeSteps +timingAvg = np.append(timingAvg, timingAvg.sum()) +timingMin = timingMat.min(axis = 0) +timingMax = timingMat.max(axis = 0) +print(timeStrings) +print(timingAvg) + +# Closing file +f.close() \ No newline at end of file From a13db79bb49a2f0f2e23d0a216c53ff99db79604 Mon Sep 17 00:00:00 2001 From: Martin Kirchengast Date: Mon, 23 May 2022 12:03:52 +0200 Subject: [PATCH 10/13] More accurate timing --- examples/OSMPDummySensor/OSMPDummySensor.cpp | 16 ++++++++-------- .../OSMPDummySensor_flat/OSMPDummySensor.cpp | 16 ++++++++-------- examples/OSMPDummySource/OSMPDummySource.cpp | 12 ++++++------ .../OSMPDummySource_flat/OSMPDummySource.cpp | 12 ++++++------ scripts/timing_analyze.py | 8 ++------ 5 files changed, 30 insertions(+), 34 deletions(-) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index 0b33eb0..ea0968f 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -274,9 +274,9 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real osi3::SensorData currentOut; double time = currentCommunicationPoint+communicationStepSize; normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); - std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSIDeserialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); if (get_fmi_sensor_view_in(currentIn)) { - std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSIDeserialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); //// Lidar Detections if (currentIn.lidar_sensor_view_size() > 0) { auto lidar_sensor = currentOut.mutable_feature_data()->add_lidar_sensor(); @@ -380,11 +380,11 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real normal_log("OSI","Mapped %d vehicles to output", i); /* Serialize */ - std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); set_fmi_sensor_data_out(currentOut); set_fmi_valid(true); set_fmi_count(currentOut.moving_object_size()); - std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); //Performance logging std::ifstream f(fileName.c_str()); @@ -432,10 +432,10 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real } size_t sensorDataSize = currentOut.ByteSize(); double osiSimTime = (double)currentOut.timestamp().seconds() + (float)currentOut.timestamp().nanos() * 0.000000001; - logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; //todo: sensor view size - logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; - logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(16) << (double)startOSIDeserialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; //todo: sensor view size + logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(16) << (double)stopOSIDeserialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << 999 << "]," << std::endl; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(16) << (double)startOSISerialize.count() << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(16) << (double)stopOSISerialize.count() << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; logFile.close(); } else { diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index 4d10c17..f700000 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -297,10 +297,10 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real DEBUGBREAK(); flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; - std::chrono::milliseconds startOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSIDeserialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); normal_log("OSI","Calculating Sensor at %f for %f (step size %f)",currentCommunicationPoint,time,communicationStepSize); const osi3::SensorView* sensor_view_in = get_fmi_sensor_view_in(); - std::chrono::milliseconds stopOSIDeserialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSIDeserialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); if (sensor_view_in) { //// Lidar Detections @@ -445,7 +445,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real } auto sensor_data = sensor_data_builder.Finish(); - std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); builder.Finish(sensor_data); auto uint8_buffer = builder.GetBufferPointer(); @@ -459,7 +459,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real set_fmi_valid(true); set_fmi_count(moving_obj_counter); - std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); //// Performance logging std::ifstream f(fileName.c_str()); @@ -508,10 +508,10 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real size_t sensorDataSize = builder.GetSize(); double osiSimTime = (double)sensor_view_in->global_ground_truth()->timestamp()->seconds() + (double)sensor_view_in->global_ground_truth()->timestamp()->nanos() * 0.000000001; - logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)startOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; - logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(13) << (double)stopOSIDeserialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; - logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(16) << (double)startOSIDeserialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; + logFile << "\t\t\t\t[" << "3" << ", " << std::setprecision(16) << (double)stopOSIDeserialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "2" << ", " << sizeof(*sensor_view_in) << "]," << std::endl; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(16) << (double)startOSISerialize.count() << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(16) << (double)stopOSISerialize.count() << ", " << osiSimTime << ", " << "1" << ", " << "5" << ", " << sensorDataSize << "]"; logFile.close(); } else { diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index cfad1bf..526e756 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -197,7 +197,7 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - std::chrono::milliseconds start_source_calc = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto start_source_calc = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); osi3::SensorView currentOut; double time = currentCommunicationPoint+communicationStepSize; @@ -339,20 +339,20 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real float osiSimTime = currentOut.global_ground_truth().timestamp().seconds() + (float)currentOut.global_ground_truth().timestamp().nanos() * 0.000000001; - std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); set_fmi_sensor_view_out(currentOut); set_fmi_valid(true); set_fmi_count(currentGT->moving_object_size()); - std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); if(fileExists) { logFile << "," << std::endl; } int sensorViewSize = currentOut.ByteSize(); - logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)start_source_calc.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(16) << (double)start_source_calc.count() << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(16) << (double)startOSISerialize.count()<< ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(16) << (double)stopOSISerialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; logFile.close(); return fmi2OK; diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index fe70bb5..97a179a 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -209,7 +209,7 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPoint) { DEBUGBREAK(); - std::chrono::milliseconds start_source_calc = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto start_source_calc = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); flatbuffers::FlatBufferBuilder builder(1024); double time = currentCommunicationPoint+communicationStepSize; @@ -343,7 +343,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real sensor_view_builder.add_lidar_sensor_view(lidar_sensor_view_flatvector); auto sensor_view = sensor_view_builder.Finish(); - std::chrono::milliseconds startOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); builder.Finish(sensor_view); auto uint8_buffer = builder.GetBufferPointer(); @@ -355,7 +355,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real set_fmi_valid(true); set_fmi_count((int)moving_object_vector.size()); - std::chrono::milliseconds stopOSISerialize = std::chrono::duration_cast< std::chrono::milliseconds >(std::chrono::system_clock::now().time_since_epoch()); + auto stopOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); //// Performance logging auto sensor_view_in = flatbuffers::GetRoot(uint8_buffer); @@ -404,9 +404,9 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real } size_t sensorViewSize = builder.GetSize(); double osiSimTime = (double)sensor_view_in->global_ground_truth()->timestamp()->seconds() + (double)sensor_view_in->global_ground_truth()->timestamp()->nanos() * 0.000000001; - logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(13) << (double)start_source_calc.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(13) << (double)startOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; - logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(13) << (double)stopOSISerialize.count()/1000.0 << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; + logFile << "\t\t\t\t[" << "0" << ", " << std::setprecision(16) << (double)start_source_calc.count() << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "1" << ", " << std::setprecision(16) << (double)startOSISerialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]," << std::endl; + logFile << "\t\t\t\t[" << "2" << ", " << std::setprecision(16) << (double)stopOSISerialize.count() << ", " << osiSimTime << ", " << "0" << ", " << "5" << ", " << sensorViewSize << "]"; logFile.close(); return fmi2OK; diff --git a/scripts/timing_analyze.py b/scripts/timing_analyze.py index d50b04f..ff159b6 100644 --- a/scripts/timing_analyze.py +++ b/scripts/timing_analyze.py @@ -1,13 +1,9 @@ import json import numpy as np -#timing_json_path = '/tmp/OSMPDummySensor_flatbuf_timing_13-30-52.json' -#timing_json_path = '/tmp/OSMPDummySource_flatbuf_timing_13-30-52.json' -#timing_json_path = '/tmp/OSMPDummySensor_flatbuf_timing_15-35-40.json' -timing_json_path = '/tmp/OSMPDummySource_flatbuf_timing_15-35-40.json' +timing_json_path = '/home/martin/development/flatbuf_test_data/flatb_builder/OSMPDummySensor_flatbuf_timing_11-18-45.json' +#timing_json_path = '/home/martin/development/flatbuf_test_data/flatb_builder/OSMPDummySource_flatbuf_timing_11-18-45.json' -#timing_json_path = '/tmp/OSMPDummySensor_Protobuf_timing_13-23-09.json' -#timing_json_path = '/tmp/OSMPDummySource_Protobuf_timing_13-23-09.json' #timing_json_path = '/tmp/OSMPDummySensor_Protobuf_timing_15-29-42.json' #timing_json_path = '/tmp/OSMPDummySource_Protobuf_timing_15-29-41.json' From 1f85dd9e504918b341d7dd940860870f17476069 Mon Sep 17 00:00:00 2001 From: Martin Kirchengast Date: Mon, 23 May 2022 16:05:43 +0200 Subject: [PATCH 11/13] Configure tests via DEFINES --- examples/OSMPDummySource/OSMPDummySource.cpp | 23 ++-- .../OSMPDummySource_flat/OSMPDummySource.cpp | 19 ++- scripts/timing_analyze.py | 129 +++++++++++------- 3 files changed, 105 insertions(+), 66 deletions(-) diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index 526e756..fb5005e 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -10,6 +10,10 @@ #include "OSMPDummySource.h" +#define NO_LIDAR_REFLECTIONS +#define LIDAR_NUM_LAYERS 32 +#define OBJECTS_MULT 1 + /* * Debug Breaks * @@ -218,6 +222,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real currentGT->mutable_host_vehicle_id()->set_value(14); //// Lidar Reflections +#ifndef NO_LIDAR_REFLECTIONS auto lidar_sensor_view = currentOut.add_lidar_sensor_view(); auto lidar_sensor_view_configuration = lidar_sensor_view->mutable_view_configuration(); lidar_sensor_view_configuration->set_field_of_view_horizontal(145.0 / 180 * M_PI); @@ -226,7 +231,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real lidar_sensor_view_configuration->mutable_mounting_position()->mutable_position()->set_y(0.0); // from middle of rear axle, in m lidar_sensor_view_configuration->mutable_mounting_position()->mutable_position()->set_z(1.232); // from middle of rear axle, in m // 0.382 + 0.85 - int no_of_layers = 32; // the number of layers of every lidar front-end + int no_of_layers = LIDAR_NUM_LAYERS; // the number of layers of every lidar front-end double azimuth_fov = 360.0; // Azimuth angle FoV in ° int rays_per_beam_vertical = 3; // vertical super-sampling factor int rays_per_beam_horizontal = 6; // horizontal super-sampling factor @@ -245,6 +250,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real ray->set_signal_strength(max_emitted_signal_strength_in_dB + 10 * std::log10(attenuation) - 10 * std::log10(rays_per_beam)); // assuming equal distribution of beam power per ray } } +#endif //// Moving Objects static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; @@ -262,12 +268,12 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real osi3::MovingObject_VehicleClassification_Type_TYPE_MOTORBIKE, osi3::MovingObject_VehicleClassification_Type_TYPE_BUS }; - for (unsigned int i=0;i<10;i++) { + for (unsigned int i=0;i<(10*OBJECTS_MULT);i++) { osi3::MovingObject *veh = currentGT->add_moving_object(); veh->mutable_id()->set_value(10+i); veh->set_type(osi3::MovingObject_Type_TYPE_VEHICLE); auto vehclass = veh->mutable_vehicle_classification(); - vehclass->set_type(source_veh_types[i]); + vehclass->set_type(source_veh_types[i % 10]); auto vehlights = vehclass->mutable_light_state(); vehlights->set_indicator_state(osi3::MovingObject_VehicleClassification_LightState_IndicatorState_INDICATOR_STATE_OFF); vehlights->set_brake_light_state(osi3::MovingObject_VehicleClassification_LightState_BrakeLightState_BRAKE_LIGHT_STATE_OFF); @@ -277,14 +283,15 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real veh->mutable_base()->mutable_dimension()->set_height(1.5); veh->mutable_base()->mutable_dimension()->set_width(2.0); veh->mutable_base()->mutable_dimension()->set_length(5.0); - veh->mutable_base()->mutable_position()->set_x(source_x_offsets[i]+time*source_x_speeds[i]); - veh->mutable_base()->mutable_position()->set_y(source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25); + const auto x_speed = source_x_speeds[i % 10]; + veh->mutable_base()->mutable_position()->set_x(source_x_offsets[i % 10]+time*x_speed); + veh->mutable_base()->mutable_position()->set_y(source_y_offsets[i % 10]+sin(time/x_speed)*0.25); veh->mutable_base()->mutable_position()->set_z(0.0); - veh->mutable_base()->mutable_velocity()->set_x(source_x_speeds[i]); - veh->mutable_base()->mutable_velocity()->set_y(cos(time/source_x_speeds[i])*0.25/source_x_speeds[i]); + veh->mutable_base()->mutable_velocity()->set_x(x_speed); + veh->mutable_base()->mutable_velocity()->set_y(cos(time/x_speed)*0.25/x_speed); veh->mutable_base()->mutable_velocity()->set_z(0.0); veh->mutable_base()->mutable_acceleration()->set_x(0.0); - veh->mutable_base()->mutable_acceleration()->set_y(-sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i])); + veh->mutable_base()->mutable_acceleration()->set_y(-sin(time/x_speed)*0.25/(x_speed*x_speed)); veh->mutable_base()->mutable_acceleration()->set_z(0.0); veh->mutable_base()->mutable_orientation()->set_pitch(0.0); veh->mutable_base()->mutable_orientation()->set_roll(0.0); diff --git a/examples/OSMPDummySource_flat/OSMPDummySource.cpp b/examples/OSMPDummySource_flat/OSMPDummySource.cpp index 97a179a..8f38d22 100644 --- a/examples/OSMPDummySource_flat/OSMPDummySource.cpp +++ b/examples/OSMPDummySource_flat/OSMPDummySource.cpp @@ -16,6 +16,10 @@ #include "OSMPDummySource.h" +#define NO_LIDAR_REFLECTIONS +#define LIDAR_NUM_LAYERS 32 +#define OBJECTS_MULT 1 + /* * Debug Breaks * @@ -219,6 +223,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real /* We act as GroundTruth Source */ //// Lidar Reflections +#ifndef NO_LIDAR_REFLECTIONS auto mounting_pos_position = osi3::CreateVector3d(builder, 1.5205, 0.0, 1.232); osi3::MountingPositionBuilder mounting_position_builder(builder); mounting_position_builder.add_position(mounting_pos_position); @@ -230,7 +235,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real lidar_sensor_view_configuration_builder.add_mounting_position(mounting_position); auto lidar_sensor_view_configuration = lidar_sensor_view_configuration_builder.Finish(); - int no_of_layers = 32; // the number of layers of every lidar front-end + int no_of_layers = LIDAR_NUM_LAYERS; // the number of layers of every lidar front-end double azimuth_fov = 360.0; // Azimuth angle FoV in ° int rays_per_beam_vertical = 3; // vertical super-sampling factor int rays_per_beam_horizontal = 6; // horizontal super-sampling factor @@ -261,6 +266,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real std::vector> lidar_sensor_view_vector; lidar_sensor_view_vector.push_back(lidar_sensor_view); auto lidar_sensor_view_flatvector = builder.CreateVector(lidar_sensor_view_vector); +#endif //// Moving Objects static double source_y_offsets[10] = { 3.0, 3.0, 3.0, 0.25, 0, -0.25, -3.0, -3.0, -3.0, -3.0 }; @@ -279,7 +285,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real osi3::MovingObject_::VehicleClassification_::Type::TYPE_BUS }; std::vector> moving_object_vector; - for (unsigned int i=0;i<10;i++) { + for (unsigned int i=0;i<(10*OBJECTS_MULT);i++) { osi3::IdentifierBuilder id_builder(builder); id_builder.add_value(10+i); auto moving_obj_id = id_builder.Finish(); @@ -294,9 +300,10 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real auto vehicle_attributes = vehicle_attributes_builder.Finish(); auto dimension = osi3::CreateDimension3d(builder, 5.0, 2.0, 1.5); - auto position = osi3::CreateVector3d(builder, source_x_offsets[i]+time*source_x_speeds[i], source_y_offsets[i]+sin(time/source_x_speeds[i])*0.25, 0.0); - auto velocity = osi3::CreateVector3d(builder, source_x_speeds[i], cos(time/source_x_speeds[i])*0.25/source_x_speeds[i], 0.0); - auto acceleration = osi3::CreateVector3d(builder, 0.0, -sin(time/source_x_speeds[i])*0.25/(source_x_speeds[i]*source_x_speeds[i]), 0.0); + const auto x_speed = source_x_speeds[i % 10]; + auto position = osi3::CreateVector3d(builder, source_x_offsets[i % 10]+time*x_speed, source_y_offsets[i % 10]+sin(time/x_speed)*0.25, 0.0); + auto velocity = osi3::CreateVector3d(builder, x_speed, cos(time/x_speed)*0.25/x_speed, 0.0); + auto acceleration = osi3::CreateVector3d(builder, 0.0, -sin(time/x_speed)*0.25/(x_speed*x_speed), 0.0); auto orientation = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); auto orientation_rate = osi3::CreateOrientation3d(builder, 0.0, 0.0, 0.0); osi3::BaseMovingBuilder base_moving_builder(builder); @@ -340,7 +347,9 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real sensor_view_builder.add_host_vehicle_id(host_vehicle_id); sensor_view_builder.add_timestamp(timestamp); sensor_view_builder.add_global_ground_truth(ground_truth); +#ifndef NO_LIDAR_REFLECTIONS sensor_view_builder.add_lidar_sensor_view(lidar_sensor_view_flatvector); +#endif auto sensor_view = sensor_view_builder.Finish(); auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); diff --git a/scripts/timing_analyze.py b/scripts/timing_analyze.py index ff159b6..0ca6d2b 100644 --- a/scripts/timing_analyze.py +++ b/scripts/timing_analyze.py @@ -1,56 +1,79 @@ import json import numpy as np +import os -timing_json_path = '/home/martin/development/flatbuf_test_data/flatb_builder/OSMPDummySensor_flatbuf_timing_11-18-45.json' -#timing_json_path = '/home/martin/development/flatbuf_test_data/flatb_builder/OSMPDummySource_flatbuf_timing_11-18-45.json' - -#timing_json_path = '/tmp/OSMPDummySensor_Protobuf_timing_15-29-42.json' -#timing_json_path = '/tmp/OSMPDummySource_Protobuf_timing_15-29-41.json' - -# Opening JSON file -f = open(timing_json_path) - -# returns JSON object as -# a dictionary -data = json.load(f) - -if 'OSMPDummySensor' in data['Data'][0]['Instance']['ModelIdentity']: - startEvent = 2 - nextEvent = 2 - eventCount = int(4) - timeStrings = ['Model deserialize', 'Model calculation', 'Model serialize', 'Total'] -else: - startEvent = 0 - nextEvent = 0 - eventCount = int(3) - timeStrings = ['Source generate', 'Source serialize', 'Total'] - -numTimeSteps = len(data['Data'][0]['OsiEvents']) // eventCount -timingMat = np.zeros((numTimeSteps, eventCount - 1)) -matIdx = 0 -prevTimeStamp = 0 - -# Iterating through the json -# list -for event in data['Data'][0]['OsiEvents']: - if event[0] != nextEvent: - print('Error in log event sequence') - break - curTimeStamp = event[1] - if event[0] != startEvent: - timingMat[np.unravel_index(matIdx, timingMat.shape)] = curTimeStamp - prevTimeStamp - matIdx += 1 - - nextEvent += 1 - nextEvent = nextEvent % eventCount - prevTimeStamp = curTimeStamp - -timingAvg = timingMat.sum(axis = 0) / numTimeSteps -timingAvg = np.append(timingAvg, timingAvg.sum()) -timingMin = timingMat.min(axis = 0) -timingMax = timingMat.max(axis = 0) -print(timeStrings) -print(timingAvg) - -# Closing file -f.close() \ No newline at end of file +############### FLATB BUILDER ################ +json_directory = '/home/martin/development/flatbuf_test_data/objects_10/flatb_builder' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_200/flatb_builder' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_500/flatb_builder' +#json_directory = '/home/martin/development/flatbuf_test_data/lidar_830k/flatb_builder' + +############### FLATB OBJECT API ############# +#json_directory = '/home/martin/development/flatbuf_test_data/objects_10/flatb_object_api' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_200/flatb_object_api' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_500/flatb_object_api' +#json_directory = '/home/martin/development/flatbuf_test_data/lidar_830k/flatb_object_api' + +############### PROTOB ####################### +#json_directory = '/home/martin/development/flatbuf_test_data/objects_10/protob' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_200/protob' +#json_directory = '/home/martin/development/flatbuf_test_data/objects_500/protob' +#json_directory = '/home/martin/development/flatbuf_test_data/lidar_830k/protob' + +def analyze_file(timing_json_path): + # Opening JSON file + f = open(timing_json_path) + + # returns JSON object as + # a dictionary + data = json.load(f) + + if 'OSMPDummySensor' in data['Data'][0]['Instance']['ModelIdentity']: + startEvent = 2 + nextEvent = 2 + eventCount = int(4) + timeStrings = ['Model deserialize', 'Model calculation', 'Model serialize', 'Total'] + else: + startEvent = 0 + nextEvent = 0 + eventCount = int(3) + timeStrings = ['Source generate', 'Source serialize', 'Total'] + + numTimeSteps = len(data['Data'][0]['OsiEvents']) // eventCount + timingMat = np.zeros((numTimeSteps, eventCount - 1)) + matIdx = 0 + prevTimeStamp = 0 + + # Iterating through the json + # list + for event in data['Data'][0]['OsiEvents']: + if event[0] != nextEvent: + print('Error in log event sequence') + break + curTimeStamp = event[1] + if event[0] != startEvent: + timingMat[np.unravel_index(matIdx, timingMat.shape)] = curTimeStamp - prevTimeStamp + matIdx += 1 + + nextEvent += 1 + nextEvent = nextEvent % eventCount + prevTimeStamp = curTimeStamp + + timingAvg = timingMat.sum(axis = 0) / numTimeSteps + timingAvg = np.append(timingAvg, timingAvg.sum()) + timingMin = timingMat.min(axis = 0) + timingMax = timingMat.max(axis = 0) + print(timeStrings) + print(timingAvg) + + # Closing file + f.close() + +def main(): + json_files=os.listdir(json_directory) + for json_file in json_files: + json_path = os.path.join(json_directory, json_file) + analyze_file(json_path) + +if __name__ == "__main__": + main() \ No newline at end of file From f4568bd6a39198ed77102af1a5aa1968324344ae Mon Sep 17 00:00:00 2001 From: Martin Kirchengast Date: Mon, 30 May 2022 13:29:20 +0200 Subject: [PATCH 12/13] DummySensor without lidar consideration --- examples/OSMPDummySensor/OSMPDummySensor.cpp | 7 ++++--- examples/OSMPDummySensor_flat/OSMPDummySensor.cpp | 9 ++++++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index ea0968f..67c3eda 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -10,6 +10,8 @@ #include "OSMPDummySensor.h" +#define NO_LIDAR_REFLECTIONS + /* * Debug Breaks * @@ -278,11 +280,10 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real if (get_fmi_sensor_view_in(currentIn)) { auto stopOSIDeserialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); //// Lidar Detections +#ifndef NO_LIDAR_DETECTIONS if (currentIn.lidar_sensor_view_size() > 0) { auto lidar_sensor = currentOut.mutable_feature_data()->add_lidar_sensor(); - int no_of_layers = 32; // the number of layers of every lidar front-end double azimuth_fov = 360.0; // Azimuth angle FoV in ° - int rays_per_beam_vertical = 3; // vertical super-sampling factor int rays_per_beam_horizontal = 6; // horizontal super-sampling factor 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 double beam_step_elevation = 0.3; // simplified equidistant beam spacing @@ -307,7 +308,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real } } } - +#endif //// Moving Objects double ego_x=0, ego_y=0, ego_z=0; osi3::Identifier ego_id = currentIn.global_ground_truth().host_vehicle_id(); diff --git a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp index f700000..443621f 100644 --- a/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor_flat/OSMPDummySensor.cpp @@ -16,6 +16,8 @@ #include "OSMPDummySensor.h" +#define NO_LIDAR_REFLECTIONS + /* * Debug Breaks * @@ -304,11 +306,10 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real if (sensor_view_in) { //// Lidar Detections +#ifndef NO_LIDAR_REFLECTIONS std::vector> lidar_detection_vector; if (sensor_view_in->lidar_sensor_view()) { - int no_of_layers = 32; // the number of layers of every lidar front-end double azimuth_fov = 360.0; // Azimuth angle FoV in ° - int rays_per_beam_vertical = 3; // vertical super-sampling factor int rays_per_beam_horizontal = 6; // horizontal super-sampling factor 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 double beam_step_elevation = 0.3; // simplified equidistant beam spacing @@ -343,7 +344,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real auto feature_data_builder = osi3::FeatureDataBuilder(builder); feature_data_builder.add_lidar_sensor(lidar_sensor_vector_flatvector); auto feature_data = feature_data_builder.Finish(); - +#endif //// Moving Objects double ego_x=0, ego_y=0, ego_z=0; const osi3::Identifier* ego_id = sensor_view_in->global_ground_truth()->host_vehicle_id(); @@ -440,9 +441,11 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real sensor_data_builder.add_timestamp(timestamp); //sensor_data_builder.add_version(interface_version); sensor_data_builder.add_moving_object(detected_moving_object_flatvector); +#ifndef NO_LIDAR_REFLECTIONS if (sensor_view_in->lidar_sensor_view()) { sensor_data_builder.add_feature_data(feature_data); } +#endif auto sensor_data = sensor_data_builder.Finish(); auto startOSISerialize = std::chrono::duration_cast< std::chrono::microseconds >(std::chrono::system_clock::now().time_since_epoch()); From b224e89a5a4aab731503fb50ea2aa1342d4b3228 Mon Sep 17 00:00:00 2001 From: Jonas Ruebsam Date: Tue, 29 Nov 2022 10:38:09 +0100 Subject: [PATCH 13/13] Fix build for windows --- examples/OSMPDummySensor/CMakeLists.txt | 2 +- examples/OSMPDummySensor/OSMPDummySensor.cpp | 2 ++ examples/OSMPDummySource/CMakeLists.txt | 2 +- examples/OSMPDummySource/OSMPDummySource.cpp | 8 ++++++-- 4 files changed, 10 insertions(+), 4 deletions(-) diff --git a/examples/OSMPDummySensor/CMakeLists.txt b/examples/OSMPDummySensor/CMakeLists.txt index b926610..625725d 100644 --- a/examples/OSMPDummySensor/CMakeLists.txt +++ b/examples/OSMPDummySensor/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySensor) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index 67c3eda..78366a5 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -54,6 +54,8 @@ #include #include #include //included for windows compatibility +#define M_PI 3.14159265358979323846264338327950288 + using namespace std; diff --git a/examples/OSMPDummySource/CMakeLists.txt b/examples/OSMPDummySource/CMakeLists.txt index bbef886..e7c2f48 100644 --- a/examples/OSMPDummySource/CMakeLists.txt +++ b/examples/OSMPDummySource/CMakeLists.txt @@ -3,7 +3,7 @@ project(OSMPDummySource) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(LINK_WITH_SHARED_OSI ON CACHE BOOL "Link FMU with shared OSI library instead of statically linking") +set(LINK_WITH_SHARED_OSI OFF CACHE BOOL "Link FMU with shared OSI library instead of statically linking") set(PUBLIC_LOGGING OFF CACHE BOOL "Enable logging via FMI logger") set(PRIVATE_LOGGING OFF CACHE BOOL "Enable private logging to file") if(WIN32) diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index fb5005e..0ec4172 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -8,11 +8,15 @@ * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ +#ifndef M_PI + #define M_PI 3.14159265358979323846 +#endif + #include "OSMPDummySource.h" #define NO_LIDAR_REFLECTIONS -#define LIDAR_NUM_LAYERS 32 -#define OBJECTS_MULT 1 +#define LIDAR_NUM_LAYERS 0 +#define OBJECTS_MULT 50 /* * Debug Breaks