Commit 7ce726f1 authored by Robin Caloudis's avatar Robin Caloudis Committed by Reinhard Biegel
Browse files

fix(Algorithm_FmuWrapper): Fix preprocessor directives w.r.t. osi 3.3.1

parent 174c3e2f
......@@ -160,7 +160,6 @@ OsmpFmuHandler::OsmpFmuHandler(fmu_check_data_t *cdata, WorldInterface *world, A
{
writeTraceGroundTruth = writeTraceGroundTruthFlag->second;
}
#ifdef USE_EXTENDED_OSI
auto writeTrafficCommandFlag = parameters->GetParametersBool().find("WriteJson_TrafficCommand");
if (writeTrafficCommandFlag != parameters->GetParametersBool().end())
{
......@@ -181,6 +180,7 @@ OsmpFmuHandler::OsmpFmuHandler(fmu_check_data_t *cdata, WorldInterface *world, A
{
writeTraceTrafficUpdate = writeTraceTrafficUpdateFlag->second;
}
#ifdef USE_EXTENDED_OSI
auto writeMotionCommandFlag = parameters->GetParametersBool().find("WriteJson_MotionCommand");
if (writeMotionCommandFlag != parameters->GetParametersBool().end())
{
......@@ -205,8 +205,8 @@ OsmpFmuHandler::OsmpFmuHandler(fmu_check_data_t *cdata, WorldInterface *world, A
bool writeJsonOutput = writeSensorData || writeSensorView || writeSensorViewConfig || writeSensorViewConfigRequest || writeTrafficCommand || writeTrafficUpdate || writeMotionCommand || writeVehicleCommunicationData || writeGroundTruth;
bool writeTraceOutput = writeTraceSensorData || writeTraceSensorView || writeTraceSensorViewConfig || writeTraceSensorViewConfigRequest || writeTraceTrafficCommand || writeTraceTrafficUpdate || writeTraceMotionCommand || writeTraceVehicleCommunicationData || writeTraceGroundTruth;
#else
bool writeJsonOutput = writeSensorData || writeSensorView || writeGroundTruth;
bool writeTraceOutput = writeTraceSensorData || writeTraceSensorView || writeTraceGroundTruth;
bool writeJsonOutput = writeSensorData || writeSensorView || writeSensorViewConfig || writeSensorViewConfigRequest || writeTrafficCommand || writeTrafficUpdate || writeGroundTruth;
bool writeTraceOutput = writeTraceSensorData || writeTraceSensorView || writeTraceSensorViewConfig || writeTraceSensorViewConfigRequest || writeTraceTrafficCommand || writeTraceTrafficUpdate || writeTraceGroundTruth;
#endif
if (writeJsonOutput)
......@@ -306,7 +306,6 @@ void OsmpFmuHandler::UpdateInput(int localLinkId, const std::shared_ptr<const Si
sensorDataIn = signal->sensorData;
}
#ifdef USE_EXTENDED_OSI
trafficCommands.try_emplace(time, std::make_unique<osi3::TrafficCommand>());
osi3::utils::SetTimestamp(*trafficCommands[time], time);
osi3::utils::SetVersion(*trafficCommands[time]);
......@@ -367,7 +366,6 @@ void OsmpFmuHandler::UpdateInput(int localLinkId, const std::shared_ptr<const Si
}
}
}
#endif
}
constexpr double EPSILON = 0.001;
......@@ -379,7 +377,7 @@ void OsmpFmuHandler::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterfa
{
data = std::make_shared<SensorDataSignal const>(sensorDataOut);
}
#ifdef USE_EXTENDED_OSI
else if (localLinkId == 0)
{
double acceleration{0.0};
......@@ -394,25 +392,7 @@ void OsmpFmuHandler::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterfa
double centripetalAcceleration{0.0};
double travelDistance{0.0};
if (motionCommandVariable.has_value())
{
setlevel4to5::DynamicState dynamicState;
if(motionCommand.trajectory().trajectory_point_size() > 0)
{
dynamicState = motionCommand.trajectory().trajectory_point(0);
}
else
{
dynamicState = motionCommand.current_state();
}
acceleration = dynamicState.acceleration();
velocity = dynamicState.velocity();
positionX = dynamicState.position_x() - bb_center_offset_x * std::cos(yaw);
positionY = dynamicState.position_y() - bb_center_offset_x * std::sin(yaw);
yaw = dynamicState.heading_angle();
}
else if (trafficUpdateVariable.has_value())
if (trafficUpdateVariable.has_value())
{
if (trafficUpdate.update_size() > 0)
{
......@@ -432,6 +412,26 @@ void OsmpFmuHandler::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterfa
LOGWARN(log_prefix(agentIdString) + "Received empty TrafficUpdate message");
}
}
#ifdef USE_EXTENDED_OSI
else if (motionCommandVariable.has_value())
{
setlevel4to5::DynamicState dynamicState;
if(motionCommand.trajectory().trajectory_point_size() > 0)
{
dynamicState = motionCommand.trajectory().trajectory_point(0);
}
else
{
dynamicState = motionCommand.current_state();
}
acceleration = dynamicState.acceleration();
velocity = dynamicState.velocity();
positionX = dynamicState.position_x() - bb_center_offset_x * std::cos(yaw);
positionY = dynamicState.position_y() - bb_center_offset_x * std::sin(yaw);
yaw = dynamicState.heading_angle();
}
#endif
else
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "Cannot provide DynamicsSignal, as neither TrafficUpdate nor MotionCommand are connected");
......@@ -461,7 +461,6 @@ void OsmpFmuHandler::UpdateOutput(int localLinkId, std::shared_ptr<SignalInterfa
centripetalAcceleration,
travelDistance);
}
#endif
}
void OsmpFmuHandler::Init()
......@@ -563,7 +562,6 @@ void OsmpFmuHandler::PreStep(int time)
time, fileToOutputTracesMap);
}
}
#ifdef USE_EXTENDED_OSI
if (trafficCommandVariable)
{
SetTrafficCommandInput(trafficCommand);
......@@ -578,6 +576,7 @@ void OsmpFmuHandler::PreStep(int time)
time, fileToOutputTracesMap);
}
}
#ifdef USE_EXTENDED_OSI
if (vehicleCommunicationDataVariable)
{
auto hostVehicleData = vehicleCommunicationData.mutable_host_vehicle_data();
......@@ -632,7 +631,7 @@ void OsmpFmuHandler::PostStep(int time)
QString::fromStdString(fmuName), time, fileToOutputTracesMap);
}
}
#endif
if (trafficUpdateVariable)
{
GetTrafficUpdate();
......@@ -647,7 +646,6 @@ void OsmpFmuHandler::PostStep(int time)
QString::fromStdString(fmuName), time, fileToOutputTracesMap);
}
}
#endif
}
FmuHandlerInterface::FmuValue& OsmpFmuHandler::GetValue(fmi2_value_reference_t valueReference, VariableType variableType) const
......@@ -713,60 +711,6 @@ void OsmpFmuHandler::GetSensorData()
sensorDataOut.ParseFromArray(buffer, GetValue(fmuVariables.at(sensorDataOutVariable.value()+".size").first, VariableType::Int).intValue);
}
#ifdef USE_EXTENDED_OSI
void OsmpFmuHandler::SetTrafficCommandInput(const osi3::TrafficCommand& data)
{
std::swap(serializedTrafficCommand, previousSerializedTrafficCommand);
fmi2_integer_t fmuInputValues[3];
fmi2_value_reference_t valueReferences[3] = {fmuVariables.at(trafficCommandVariable.value()+".base.lo").first,
fmuVariables.at(trafficCommandVariable.value()+".base.hi").first,
fmuVariables.at(trafficCommandVariable.value()+".size").first};
data.SerializeToString(&serializedTrafficCommand);
encode_pointer_to_integer(serializedTrafficCommand.data(),
fmuInputValues[1],
fmuInputValues[0]);
auto length = serializedTrafficCommand.length();
if (length > std::numeric_limits<fmi2_integer_t>::max())
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "Serialized buffer length of osi::TrafficCommand exceeds fmi2 integer size");
}
fmuInputValues[2] = static_cast<fmi2_integer_t>(length);
fmi2_import_set_integer(cdata->fmu2,
valueReferences, // array of value reference
3, // number of elements
fmuInputValues); // array of values
}
void OsmpFmuHandler::SetVehicleCommunicationDataInput(const setlevel4to5::VehicleCommunicationData& data)
{
std::swap(serializedVehicleCommunicationData, previousSerializedVehicleCommunicationData);
fmi2_integer_t fmuInputValues[3];
fmi2_value_reference_t valueReferences[3] = {fmuVariables.at(vehicleCommunicationDataVariable.value()+".base.lo").first,
fmuVariables.at(vehicleCommunicationDataVariable.value()+".base.hi").first,
fmuVariables.at(vehicleCommunicationDataVariable.value()+".size").first};
data.SerializeToString(&serializedVehicleCommunicationData);
encode_pointer_to_integer(serializedVehicleCommunicationData.data(),
fmuInputValues[1],
fmuInputValues[0]);
fmuInputValues[2] = serializedVehicleCommunicationData.length();
fmi2_import_set_integer(cdata->fmu2,
valueReferences, // array of value reference
3, // number of elements
fmuInputValues); // array of values
}
void logAndThrow(const std::string& message, const std::function<void(const std::string&)> &errorCallback) noexcept(false) {
if (errorCallback) errorCallback(message);
throw std::runtime_error(message);
}
void OsmpFmuHandler::AddTrafficCommandActionFromOpenScenarioTrajectory(osi3::TrafficAction *trafficAction, const openScenario::Trajectory& trajectory)
{
if (trajectory.timeReference.has_value()) {
......@@ -835,6 +779,89 @@ void OsmpFmuHandler::AddTrafficCommandActionFromOpenScenarioPosition(osi3::Traff
}},
position);
}
void OsmpFmuHandler::SetTrafficCommandInput(const osi3::TrafficCommand& data)
{
std::swap(serializedTrafficCommand, previousSerializedTrafficCommand);
fmi2_integer_t fmuInputValues[3];
fmi2_value_reference_t valueReferences[3] = {fmuVariables.at(trafficCommandVariable.value()+".base.lo").first,
fmuVariables.at(trafficCommandVariable.value()+".base.hi").first,
fmuVariables.at(trafficCommandVariable.value()+".size").first};
data.SerializeToString(&serializedTrafficCommand);
encode_pointer_to_integer(serializedTrafficCommand.data(),
fmuInputValues[1],
fmuInputValues[0]);
auto length = serializedTrafficCommand.length();
if (length > std::numeric_limits<fmi2_integer_t>::max())
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "Serialized buffer length of osi::TrafficCommand exceeds fmi2 integer size");
}
fmuInputValues[2] = static_cast<fmi2_integer_t>(length);
fmi2_import_set_integer(cdata->fmu2,
valueReferences, // array of value reference
3, // number of elements
fmuInputValues); // array of values
}
void OsmpFmuHandler::GetTrafficUpdate()
{
void *buffer = decode_integer_to_pointer(GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".base.hi").first, VariableType::Int).intValue,
GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".base.lo").first, VariableType::Int).intValue);
if (enforceDoubleBuffering && buffer != nullptr && buffer == previousTrafficUpdate)
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "FMU has no double buffering");
}
previousTrafficUpdate = buffer;
trafficUpdate.ParseFromArray(buffer, GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".size").first, VariableType::Int).intValue);
trafficUpdate.SerializeToString(&serializedTrafficUpdate);
}
#ifdef USE_EXTENDED_OSI
void OsmpFmuHandler::SetVehicleCommunicationDataInput(const setlevel4to5::VehicleCommunicationData& data)
{
std::swap(serializedVehicleCommunicationData, previousSerializedVehicleCommunicationData);
fmi2_integer_t fmuInputValues[3];
fmi2_value_reference_t valueReferences[3] = {fmuVariables.at(vehicleCommunicationDataVariable.value()+".base.lo").first,
fmuVariables.at(vehicleCommunicationDataVariable.value()+".base.hi").first,
fmuVariables.at(vehicleCommunicationDataVariable.value()+".size").first};
data.SerializeToString(&serializedVehicleCommunicationData);
encode_pointer_to_integer(serializedVehicleCommunicationData.data(),
fmuInputValues[1],
fmuInputValues[0]);
fmuInputValues[2] = serializedVehicleCommunicationData.length();
fmi2_import_set_integer(cdata->fmu2,
valueReferences, // array of value reference
3, // number of elements
fmuInputValues); // array of values
}
void logAndThrow(const std::string& message, const std::function<void(const std::string&)> &errorCallback) noexcept(false) {
if (errorCallback) errorCallback(message);
throw std::runtime_error(message);
}
void OsmpFmuHandler::GetMotionCommand()
{
void* buffer = decode_integer_to_pointer(GetValue(fmuVariables.at(motionCommandVariable.value()+".base.hi").first, VariableType::Int).intValue,
GetValue(fmuVariables.at(motionCommandVariable.value()+".base.lo").first, VariableType::Int).intValue);
if (enforceDoubleBuffering && buffer != nullptr && buffer == previousMotionCommand)
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "FMU has no double buffering");
}
previousMotionCommand = buffer;
motionCommand.ParseFromArray(buffer, GetValue(fmuVariables.at(motionCommandVariable.value()+".size").first, VariableType::Int).intValue);
}
#endif
template <typename GetParametersType, typename FmuTypeParameters, typename UnderlyingType>
......@@ -998,20 +1025,6 @@ void OsmpFmuHandler::SetFmuParameters()
}
#ifdef USE_EXTENDED_OSI
void OsmpFmuHandler::GetMotionCommand()
{
void* buffer = decode_integer_to_pointer(GetValue(fmuVariables.at(motionCommandVariable.value()+".base.hi").first, VariableType::Int).intValue,
GetValue(fmuVariables.at(motionCommandVariable.value()+".base.lo").first, VariableType::Int).intValue);
if (enforceDoubleBuffering && buffer != nullptr && buffer == previousMotionCommand)
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "FMU has no double buffering");
}
previousMotionCommand = buffer;
motionCommand.ParseFromArray(buffer, GetValue(fmuVariables.at(motionCommandVariable.value()+".size").first, VariableType::Int).intValue);
}
void OsmpFmuHandler::SetMotionCommandDataInput(const setlevel4to5::MotionCommand &data) {
std::swap(serializedMotionCommand, previousSerializedMotionCommand);
fmi2_integer_t fmuInputValues[3];
......@@ -1030,23 +1043,6 @@ void OsmpFmuHandler::SetMotionCommandDataInput(const setlevel4to5::MotionCommand
3, // number of elements
fmuInputValues); // array of values
}
void OsmpFmuHandler::GetTrafficUpdate()
{
void *buffer = decode_integer_to_pointer(GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".base.hi").first, VariableType::Int).intValue,
GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".base.lo").first, VariableType::Int).intValue);
if (enforceDoubleBuffering && buffer != nullptr && buffer == previousTrafficUpdate)
{
LOGERRORANDTHROW(log_prefix(agentIdString) + "FMU has no double buffering");
}
previousTrafficUpdate = buffer;
trafficUpdate.ParseFromArray(buffer, GetValue(fmuVariables.at(trafficUpdateVariable.value() + ".size").first, VariableType::Int).intValue);
trafficUpdate.SerializeToString(&serializedTrafficUpdate);
}
#endif
void OsmpFmuHandler::WriteJson(const google::protobuf::Message& message, const QString& fileName)
......
......@@ -21,10 +21,10 @@
#include "osi3/osi_groundtruth.pb.h"
#include "osi3/osi_sensordata.pb.h"
#include "osi3/osi_sensorview.pb.h"
#ifdef USE_EXTENDED_OSI
#include "osi3/osi_trafficcommand.pb.h"
#include "osi3/osi_trafficupdate.pb.h"
#ifdef USE_EXTENDED_OSI
#include "osi3/sl45_motioncommand.pb.h"
#include "osi3/sl45_vehiclecommunicationdata.pb.h"
#endif
......@@ -66,7 +66,6 @@ public:
//! Reads an output value of the FMU
FmuHandlerInterface::FmuValue& GetValue(fmi2_value_reference_t valueReference, VariableType variableType) const;
#ifdef USE_EXTENDED_OSI
//! Adds a trajectory from OpenSCENARIO to a OSI TrafficAction
static void AddTrafficCommandActionFromOpenScenarioTrajectory(osi3::TrafficAction *trafficAction,
const openScenario::Trajectory& trajectory);
......@@ -76,8 +75,6 @@ public:
const openScenario::Position &position,
WorldInterface *worldInterface,
const std::function<void(const std::string &)> &errorCallback);
#endif
//! Parameter of the FMU that was defined in the profile
template <typename T>
......@@ -127,21 +124,22 @@ private:
//! Sets the SensorViewConfigRequest
void SetSensorViewConfigRequest();
#ifdef USE_EXTENDED_OSI
//! Sets the TrafficCommand as input for the FMU
void SetTrafficCommandInput(const osi3::TrafficCommand &data);
//! Sets the VehicleCommunicationData as input for the FMU
void SetVehicleCommunicationDataInput(const setlevel4to5::VehicleCommunicationData &data);
//! Reads the TrafficUpdate from the FMU
void GetTrafficUpdate();
#ifdef USE_EXTENDED_OSI
//! Reads the MotionCommand from the FMU
void GetMotionCommand();
//! Sets the VehicleCommunicationData as input for the FMU
void SetVehicleCommunicationDataInput(const setlevel4to5::VehicleCommunicationData &data);
//! Sets the MotionCommand as input for the FMU (makes it also available for json output, this needs refactoring)
void SetMotionCommandDataInput(const setlevel4to5::MotionCommand &data);
//! Reads the TrafficUpdate from the FMU
void GetTrafficUpdate();
#endif
//! Writes an OSI message into a JSON file
......@@ -179,19 +177,18 @@ private:
osi3::SensorData sensorDataOut;
std::string serializedGroundTruth;
std::string appendedSerializedGroundTruth;
#ifdef USE_EXTENDED_OSI
std::string serializedTrafficCommand;
std::string appendedSerializedTrafficCommand;
std::string previousSerializedTrafficCommand;
std::string serializedVehicleCommunicationData;
std::string appendedSerializedVehicleCommunicationData;
std::string previousSerializedVehicleCommunicationData;
osi3::TrafficUpdate trafficUpdate;
void* previousTrafficUpdate{nullptr};
osi3::TrafficCommand trafficCommand;
std::map<int, std::unique_ptr<osi3::TrafficCommand>> trafficCommands{};
#ifdef USE_EXTENDED_OSI
std::string serializedVehicleCommunicationData;
std::string appendedSerializedVehicleCommunicationData;
std::string previousSerializedVehicleCommunicationData;
setlevel4to5::VehicleCommunicationData vehicleCommunicationData;
setlevel4to5::MotionCommand motionCommand;
std::string previousSerializedMotionCommand;
......@@ -202,18 +199,17 @@ private:
void* previousMotionCommand{nullptr};
#endif
std::optional<std::string> sensorViewVariable;
std::optional<std::string> sensorViewConfigVariable;
std::optional<std::string> sensorViewConfigRequestVariable;
std::optional<std::string> sensorDataInVariable;
std::optional<std::string> sensorDataOutVariable;
std::optional<std::string> groundTruthVariable;
std::optional<std::string> trafficCommandVariable;
std::optional<std::string> trafficUpdateVariable;
#ifdef USE_EXTENDED_OSI
std::optional<std::string> motionCommandVariable;
std::optional<std::string> trafficCommandVariable;
std::optional<std::string> trafficUpdateVariable;
std::optional<std::string> vehicleCommunicationDataVariable;
#endif
......@@ -222,17 +218,17 @@ private:
{{"SensorView", sensorViewVariable},
{"SensorViewConfig", sensorViewConfigVariable},
{"SensorData", sensorDataInVariable},
#ifdef USE_EXTENDED_OSI
{"TrafficCommand", trafficCommandVariable},
#ifdef USE_EXTENDED_OSI
{"VehicleCommunicationData", vehicleCommunicationDataVariable}
#endif
}},
{"Output",
{{"SensorData", sensorDataOutVariable},
{"SensorViewConfigRequest", sensorViewConfigRequestVariable},
{"TrafficUpdate", trafficUpdateVariable},
#ifdef USE_EXTENDED_OSI
{"MotionCommand", motionCommandVariable},
{"TrafficUpdate", trafficUpdateVariable}
{"MotionCommand", motionCommandVariable}
#endif
}},
{"Init",
......@@ -261,21 +257,21 @@ private:
bool writeSensorViewConfigRequest{false};
bool writeSensorData{false};
bool writeGroundTruth{false};
bool writeTrafficCommand{false};
bool writeTrafficUpdate{false};
bool writeTraceSensorView{false};
bool writeTraceSensorViewConfig{false};
bool writeTraceSensorViewConfigRequest{false};
bool writeTraceSensorData{false};
bool writeTraceGroundTruth{false};
bool writeTraceTrafficCommand{false};
bool writeTraceTrafficUpdate{false};
#ifdef USE_EXTENDED_OSI
bool writeTrafficCommand{false};
bool writeTrafficUpdate{false};
bool writeMotionCommand{false};
bool writeVehicleCommunicationData{false};
bool writeTraceTrafficCommand{false};
bool writeTraceTrafficUpdate{false};
bool writeTraceMotionCommand{false};
bool writeTraceVehicleCommunicationData{false};
#endif
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment