Skip to content

Commit

Permalink
Add auto-configuration to OSMPDummySensor example (#38)
Browse files Browse the repository at this point in the history 
* Remove cruft from spec for release, cleanup

* Minor code cleanup for dummy source

* Add auto configuration to OSMPDummySensor example
  • Loading branch information
@pmai @jdsika
pmai authored and jdsika committed Jun 12, 2018
1 parent 14852f5 commit f112c18
Show file tree
Hide file tree
Showing 7 changed files with 194 additions and 54 deletions.
7 changes: 7 additions & 0 deletions .markdownlint.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
{
"default": true,
"MD003": { "style": "atx" },
"MD007": { "indent": 4 },
"MD013": false,
"MD030": { "ol_multi": 3, "ul_multi": 3 }
}
40 changes: 17 additions & 23 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -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)

Expand Down Expand Up @@ -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.
110 changes: 92 additions & 18 deletions examples/OSMPDummySensor/OSMPDummySensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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()
Expand All @@ -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
*/
Expand All @@ -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;
}

Expand All @@ -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();
Expand All @@ -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
Expand All @@ -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());
Expand All @@ -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);
Expand All @@ -284,6 +350,7 @@ fmi2Status COSMPDummySensor::doCalc(fmi2Real currentCommunicationPoint, fmi2Real
fmi2Status COSMPDummySensor::doTerm()
{
DEBUGBREAK();

return fmi2OK;
}

Expand All @@ -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");
Expand Down Expand Up @@ -385,6 +452,7 @@ fmi2Status COSMPDummySensor::EnterInitializationMode()
fmi2Status COSMPDummySensor::ExitInitializationMode()
{
fmi_verbose_log("fmi2ExitInitializationMode()");
simulation_started = true;
return doExitInitializationMode();
}

Expand All @@ -405,6 +473,7 @@ fmi2Status COSMPDummySensor::Reset()
fmi_verbose_log("fmi2Reset()");

doFree();
simulation_started = false;
return doInit();
}

Expand All @@ -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;
Expand Down
28 changes: 23 additions & 5 deletions examples/OSMPDummySensor/OSMPDummySensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down Expand Up @@ -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();
};
Loading

0 comments on commit f112c18

Please sign in to comment.