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

Author: pmai

.markdownlint.json

@@ -0,0 +1,7 @@
+{
+    "default": true,
+    "MD003": { "style": "atx" },
+    "MD007": { "indent": 4 },
+    "MD013": false,
+    "MD030": { "ol_multi": 3, "ul_multi": 3 }
+}

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:
 </fmiModelDescription>
 ```
 
-## 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.

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(&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);
+}
+
+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(&currentBuffer);
-    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(&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()
@@ -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; i<FMI_STRING_VARS; i++)
         string_vars[i] = "";
 
+    set_fmi_nominal_range(135.0);
     return fmi2OK;
 }
 
 fmi2Status COSMPDummySensor::doStart(fmi2Boolean toleranceDefined, fmi2Real tolerance, fmi2Real startTime, fmi2Boolean stopTimeDefined, fmi2Real stopTime)
 {
     DEBUGBREAK();
-    last_time = startTime;
+
     return fmi2OK;
 }
 
 fmi2Status COSMPDummySensor::doEnterInitializationMode()
 {
+    DEBUGBREAK();
+
     return fmi2OK;
 }
 
 fmi2Status COSMPDummySensor::doExitInitializationMode()
 {
+    DEBUGBREAK();
+
+    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","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;
 }
 
@@ -194,19 +258,20 @@ void rotatePoint(double x, double y, double z,double yaw,double pitch,double rol
 fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real communicationStepSize, fmi2Boolean noSetFMUStatePriorToCurrentPointfmi2Component)
 {
     DEBUGBREAK();
+
     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);
     if (get_fmi_sensor_view_in(currentIn)) {
         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: %d",ego_id.value());
+        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 %d is EgoVehicle: %d",obj.id().value(), obj.id().value() == ego_id.value());
+                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: %d",obj.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();
@@ -224,8 +289,9 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
         currentOut.add_sensor_view()->CopyFrom(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,&currentIn,&currentOut,ego_id,ego_x,ego_y,ego_z](const osi3::MovingObject& veh) {
+            [this,&i,&currentIn,&currentOut,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<nvr; i++) {
-        if (vr[i]<FMI_INTEGER_VARS)
+        if (vr[i]<FMI_INTEGER_VARS) {
+            if (need_refresh && (vr[i] == FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASEHI_IDX || vr[i] == FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_BASELO_IDX || vr[i] == FMI_INTEGER_SENSORVIEW_CONFIG_REQUEST_SIZE_IDX)) {
+                refresh_fmi_sensor_view_config_request();
+                need_refresh = false;
+            }
             value[i] = integer_vars[vr[i]];
-        else
+        } else
             return fmi2Error;
     }
     return fmi2OK;

examples/OSMPDummySensor/OSMPDummySensor.h

@@ -53,12 +53,19 @@ using namespace std;
 #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_COUNT_IDX 6
+#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_LAST_IDX 0
+#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 */
@@ -194,18 +201,29 @@ class COSMPDummySensor {
     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;
+    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);
     void set_fmi_sensor_data_out(const osi3::SensorData& data);
     void reset_fmi_sensor_data_out();
+
+    /* Refreshing of Calculated Parameters */
+    void refresh_fmi_sensor_view_config_request();
 };