From f112c183f768a0eac3871122b1466e89ea6a5c45 Mon Sep 17 00:00:00 2001 From: "Pierre R. Mai" Date: Tue, 12 Jun 2018 18:02:59 +0200 Subject: [PATCH] Add auto-configuration to OSMPDummySensor example (#38) * Remove cruft from spec for release, cleanup * Minor code cleanup for dummy source * Add auto configuration to OSMPDummySensor example --- .markdownlint.json | 7 ++ README.md | 40 +++---- examples/OSMPDummySensor/OSMPDummySensor.cpp | 110 +++++++++++++++--- examples/OSMPDummySensor/OSMPDummySensor.h | 28 ++++- .../OSMPDummySensor/modelDescription.in.xml | 50 +++++++- examples/OSMPDummySource/OSMPDummySource.cpp | 12 +- examples/OSMPDummySource/OSMPDummySource.h | 1 - 7 files changed, 194 insertions(+), 54 deletions(-) create mode 100644 .markdownlint.json diff --git a/.markdownlint.json b/.markdownlint.json new file mode 100644 index 0000000..4042f68 --- /dev/null +++ b/.markdownlint.json @@ -0,0 +1,7 @@ +{ + "default": true, + "MD003": { "style": "atx" }, + "MD007": { "indent": 4 }, + "MD013": false, + "MD030": { "ol_multi": 3, "ul_multi": 3 } +} \ No newline at end of file diff --git a/README.md b/README.md index 80d8f6f..5302243 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ -OSI Sensor Model Packaging -========================== +# OSI Sensor Model Packaging [![Build Status](https://travis-ci.org/OpenSimulationInterface/osi-sensor-model-packaging.svg?branch=master)](https://travis-ci.org/OpenSimulationInterface/osi-sensor-model-packaging) @@ -362,26 +361,21 @@ model FMU with one input and output and no additional features: ``` -## TODOs - -- Support various raw data streams, like e.g. video streams, or raw - reflection lists, in the same vein as the current object list - sensor data. This is likely to be achieved using the same - basic mechanism, but including a string constant per input/output - indicating the MIME type of the transported data (or some other - naming scheme for the type), so that the variables can be - identified properly. Can be merged with the above mechanism for - auto configuration, if employed. - ## Future Evolution -In the future an extension to the FMI standard that directly -supported opaque binary data (e.g. a binary data type that -is defined in the same way as the current string data type, -but length terminated instead of zero-terminated) would allow -migration of sensor models using the current convention to -one where the sensor data input and output variables could be -mapped into just one such input and output variable each, of -the relevant binary type. The object lifetimes might need to -be adjusted in such a case to match the FMI standard extension, -depending on the form that is going to take. +For FMI 3.0, which is currently in development, an opaque +binary data type (a binary data type that is defined in the +same way as the current string data type, but length terminated +instead of zero-terminated) is planned to be added. This will +allow migration of sensor models using the current convention to +one where the relevant OSMP binary variables will be directly +mapped to such new binary variables, instead of relying on the +annotated trio of integer variables for each notional binary +variable as is currently specified. The life-time of the new +FMI 3.0 variables will be the standard life-time of all FMI +variables, and thus shorter than is currently specified, so +copying on input and output is going to be required. Other +than that the current specification can be mapped 1:1 onto this +new mechanism, and once FMI 3.0 is released, an updated OSMP +specification including this option and mapping will be +released. \ No newline at end of file diff --git a/examples/OSMPDummySensor/OSMPDummySensor.cpp b/examples/OSMPDummySensor/OSMPDummySensor.cpp index f030011..cade985 100644 --- a/examples/OSMPDummySensor/OSMPDummySensor.cpp +++ b/examples/OSMPDummySensor/OSMPDummySensor.cpp @@ -100,6 +100,34 @@ void encode_pointer_to_integer(const void* ptr,fmi2Integer& hi,fmi2Integer& lo) #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; + } +} + +void COSMPDummySensor::set_fmi_sensor_view_config_request(const osi3::SensorViewConfiguration& data) +{ + data.SerializeToString(¤tConfigRequestBuffer); + 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); +} + +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; +} + bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data) { if (integer_vars[FMI_INTEGER_SENSORVIEW_IN_SIZE_IDX] > 0) { @@ -114,11 +142,11 @@ bool COSMPDummySensor::get_fmi_sensor_view_in(osi3::SensorView& data) void COSMPDummySensor::set_fmi_sensor_data_out(const osi3::SensorData& data) { - data.SerializeToString(¤tBuffer); - encode_pointer_to_integer(currentBuffer.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)currentBuffer.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],currentBuffer.data()); - swap(currentBuffer,lastBuffer); + data.SerializeToString(¤tOutputBuffer); + 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() @@ -128,6 +156,27 @@ void COSMPDummySensor::reset_fmi_sensor_data_out() 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); + } +} + /* * Actual Core Content */ @@ -152,23 +201,38 @@ fmi2Status COSMPDummySensor::doInit() for (int i = 0; iCopyFrom(currentIn); 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](const osi3::MovingObject& veh) { + [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 @@ -236,11 +302,11 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real 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 <= 150.0) && (rel_x/distance > 0.866025)) { + 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((distance-75.0)/75.0)); + 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(veh.base().position().x()); @@ -255,15 +321,15 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real candidate->mutable_vehicle_classification()->CopyFrom(veh.vehicle_classification()); candidate->set_probability(1); - normal_log("OSI","Output Vehicle %d[%d] 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()); + 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[%d] 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()); + 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[%d] 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","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); @@ -284,6 +350,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real fmi2Status COSMPDummySensor::doTerm() { DEBUGBREAK(); + return fmi2OK; } @@ -304,7 +371,7 @@ COSMPDummySensor::COSMPDummySensor(fmi2String theinstanceName, fmi2Type thefmuTy functions(*thefunctions), visible(!!thevisible), loggingOn(!!theloggingOn), - last_time(0.0) + simulation_started(false) { loggingCategories.clear(); loggingCategories.insert("FMI"); @@ -385,6 +452,7 @@ fmi2Status COSMPDummySensor::EnterInitializationMode() fmi2Status COSMPDummySensor::ExitInitializationMode() { fmi_verbose_log("fmi2ExitInitializationMode()"); + simulation_started = true; return doExitInitializationMode(); } @@ -405,6 +473,7 @@ fmi2Status COSMPDummySensor::Reset() fmi_verbose_log("fmi2Reset()"); doFree(); + simulation_started = false; return doInit(); } @@ -429,10 +498,15 @@ fmi2Status COSMPDummySensor::GetReal(const fmi2ValueReference vr[], size_t nvr, fmi2Status COSMPDummySensor::GetInteger(const fmi2ValueReference vr[], size_t nvr, fmi2Integer value[]) { fmi_verbose_log("fmi2GetInteger(...)"); + bool need_refresh = !simulation_started; for (size_t i = 0; i + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + - - + + + + + + + diff --git a/examples/OSMPDummySource/OSMPDummySource.cpp b/examples/OSMPDummySource/OSMPDummySource.cpp index bc945dc..1860df6 100644 --- a/examples/OSMPDummySource/OSMPDummySource.cpp +++ b/examples/OSMPDummySource/OSMPDummySource.cpp @@ -146,17 +146,21 @@ fmi2Status COSMPDummySource::doInit() fmi2Status COSMPDummySource::doStart(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime) { DEBUGBREAK(); - last_time = startTime; + return fmi2OK; } fmi2Status COSMPDummySource::doEnterInitializationMode() { + DEBUGBREAK(); + return fmi2OK; } fmi2Status COSMPDummySource::doExitInitializationMode() { + DEBUGBREAK(); + return fmi2OK; } @@ -182,6 +186,7 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component) { DEBUGBREAK(); + osi3::SensorView currentOut; double time = currentCommunicationPoint+communicationStepSize; @@ -242,7 +247,7 @@ fmi2Status COSMPDummySource::doCalc(fmi2Real currentCommunicationPoint, fmi2Real 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[%d] 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()); + 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); @@ -273,8 +278,7 @@ COSMPDummySource::COSMPDummySource(fmi2String theinstanceName, fmi2Type thefmuTy fmuResourceLocation(thefmuResourceLocation), functions(*thefunctions), visible(!!thevisible), - loggingOn(!!theloggingOn), - last_time(0.0) + loggingOn(!!theloggingOn) { loggingCategories.clear(); loggingCategories.insert("FMI"); diff --git a/examples/OSMPDummySource/OSMPDummySource.h b/examples/OSMPDummySource/OSMPDummySource.h index dfa642d..f2ce33e 100644 --- a/examples/OSMPDummySource/OSMPDummySource.h +++ b/examples/OSMPDummySource/OSMPDummySource.h @@ -190,7 +190,6 @@ class COSMPDummySource { fmi2Integer integer_vars[FMI_INTEGER_VARS]; fmi2Real real_vars[FMI_REAL_VARS]; string string_vars[FMI_STRING_VARS]; - double last_time; string currentBuffer; string lastBuffer;