Class FMIAdapter

Class Documentation

class FMIAdapter

An instance of this class wraps a FMU and allows to simulate it using the ROS time notion and standard C++ types. In the background, the FMI Library (a BSD-licensed C library) is used for interacting with the FMU. This class also provides concenvience functions to read parameters and initial values from ROS parameters.

Public Functions

explicit FMIAdapter(rclcpp::Logger logger, const std::string &fmuPath, rclcpp::Duration stepSize = internal::ZERO_DURATION, bool interpolateInput = true, const std::string &tmpPath = "")

This ctor creates an instance using the FMU from the given path. If the step-size argument is zero, the default experiment step-size given in the FMU is used.

virtual ~FMIAdapter()
bool canHandleVariableCommunicationStepSize() const

Returns true if the FMU of this instances supports a variable communication step-size.

rclcpp::Duration getDefaultExperimentStep() const

Returns the default experiment step-size of the FMU of this instance.

std::vector<fmi2_import_variable_t*> getAllVariables() const

Returns all variables (including parameters, aliases, etc.) of the wrapped FMU in the FMI Library’s internal representation.

std::vector<fmi2_import_variable_t*> getInputVariables() const

Returns all input variables of the wrapped FMU in the FMI Library’s internal representation.

std::vector<fmi2_import_variable_t*> getOutputVariables() const

Returns all output variables of the wrapped FMU in the FMI Library’s internal representation.

std::vector<fmi2_import_variable_t*> getParameters() const

Returns all parameters of the wrapped FMU in the FMI Library’s internal representation.

std::vector<std::string> getAllVariableNames() const

Returns the names of all variables (including parameters and aliases) of the wrapped FMU in the FMI Library’s internal representation.

std::vector<std::string> getInputVariableNames() const

Returns the names of all input variables of the wrapped FMU in the FMI Library’s internal representation.

std::vector<std::string> getOutputVariableNames() const

Returns the names of all output variables of the wrapped FMU in the FMI Library’s internal representation.

std::vector<std::string> getParameterNames() const

Returns the names of all parameters of the wrapped FMU in the FMI Library’s internal representation.

void setInputValue(fmi2_import_variable_t *variable, const rclcpp::Time &time, double value)

Stores a value for the given variable to be considered by doStep*(..) at the given time of the FMU simulation.

void setInputValue(const std::string &variableName, const rclcpp::Time &time, double value)

Stores a value for the variable with the given name to be considered by doStep*(..) at the given time of the FMU simulation.

inline rclcpp::Duration getStepSize() const

Returns the step-size used in the FMU simulation.

inline bool isInInitializationMode() const

Returns true if the wrapped FMU is still in initialization mode, which allows to set parameters and initial values.

void exitInitializationMode(const rclcpp::Time &simulationTime)

Exits the initialization mode and starts the simulation of the wrapped FMU. Uses the given timestamp as start time for the simulation whereas the FMU internally starts at time 0. All times passed to setValue(..) and doStep*(..) are translated correspondingly.

rclcpp::Time doStep()

Performs one simulation step using the configured step size and returns the current simulation time.

rclcpp::Time doStep(const rclcpp::Duration &stepSize)

Performs one simulation step using the given step size and returns the current simulation time.

rclcpp::Time doStepsUntil(const rclcpp::Time &simulationTime)

Advances the simulation of the wrapped FMU until the given point in time (modulo step-size). In detail, the simulation is performed iteratively using the configured step-size. Before each simulation step the relevant input values passed previously by setInputValue(..) are set depending on the given timestamps.

rclcpp::Time getSimulationTime() const

Returns the current simulation time.

double getOutputValue(fmi2_import_variable_t *variable) const

Returns the current value of the given output variable.

double getOutputValue(const std::string &variableName) const

Returns the current value of the output variable with the given name.

double getValue(fmi2_import_variable_t *variable) const

Returns the current value of the given variable.

double getValue(const std::string &variableName) const

Returns the current value of the variable with the given name.

void setInitialValue(fmi2_import_variable_t *variable, double value)

Sets the given value of the given variable (or parameter or alias) as initial values. This function may be called only while isInInitializationMode() = true.

void setInitialValue(const std::string &variableName, double value)

Sets the given value of the variable (or parameter or alias) with the given name as initial values. This function may be called only while isInInitializationMode() = true.

void declareROSParameters(rclcpp::node_interfaces::NodeParametersInterface::SharedPtr nodeInterface)

Declares a ROS parameter for each FMU variable, parameter and alias. Note that ROS parameter names may use the characters [A-Za-z0-9_] only. Therefore, all other characters in an FMU variable name are mapped to ‘_’. For example, the FMU variable ‘dx[2]’ is named ‘/my_node_name/dx_2_’ as a ROS parameter.

void initializeFromROSParameters(rclcpp::node_interfaces::NodeParametersInterface::SharedPtr nodeInterface)

Tries to read inital values for each variable (including parameters and aliases) from the ROS parameter set. This function requires that ROS parameters have been declared for the FMU variables, parameters and aliases first using declareROSParameters(..).

Public Static Functions

static std::string rosifyName(const std::string &name)

This helper function returns a ROSified version of the given variable name for use with ROS parameters. It simply replaces all special charaters not supported by ROS with an ‘_’.