Skip to content

Commit

Permalink
actuators: implement rules/constraints
Browse files Browse the repository at this point in the history
  • Loading branch information
@bkueng
bkueng committed Nov 12, 2021
1 parent 1bcd7f6 commit fdd141e
Show file tree
Hide file tree
Showing 4 changed files with 222 additions and 17 deletions.
3 changes: 2 additions & 1 deletion src/AutoPilotPlugins/PX4/ActuatorComponent.qml
View file
Expand Up @@ -109,7 +109,8 @@ SetupPage {
fact: object.fact
Layout.row: 1 + channelIndex
Layout.column: 1 + index
visible: object.config.visible && (_showAdvanced || !object.config.advanced)
visible: object.config.visible && (_showAdvanced || !object.config.advanced) && object.visible
enabled: object.enabled
}
}
}
Expand Down
54 changes: 53 additions & 1 deletion src/Vehicle/Actuators/Actuators.cc
View file
Expand Up @@ -24,6 +24,7 @@ Actuators::Actuators(QObject* parent, Vehicle* vehicle)
_motorAssignment(nullptr, vehicle, _actuatorOutputs), _vehicle(vehicle)
{
connect(&_mixer, &Mixer::Mixers::paramChanged, this, &Actuators::parametersChanged);
connect(&_mixer, &Mixer::Mixers::geometryParamChanged, this, &Actuators::updateGeometryImage);
qRegisterMetaType<Actuators*>("Actuators*");
connect(&_motorAssignment, &MotorAssignment::activeChanged, this, &Actuators::motorAssignmentActiveChanged);
connect(&_motorAssignment, &MotorAssignment::messageChanged, this, &Actuators::motorAssignmentMessageChanged);
Expand Down Expand Up @@ -534,6 +535,7 @@ bool Actuators::parseJson(const QJsonDocument &json)
QJsonValue parameter = parameterJson.toObject();
Mixer::MixerParameter mixerParameter{};
mixerParameter.param.parse(parameter);
mixerParameter.identifier = parameter["identifier"].toString();
QString function = parameter["function"].toString();
if (function == "posx") {
mixerParameter.function = Mixer::Function::PositionX;
Expand Down Expand Up @@ -590,7 +592,57 @@ bool Actuators::parseJson(const QJsonDocument &json)
mixerOptions.append(option);
}

_mixer.reset(actuatorTypes, mixerOptions, outputFunctions);
QList<Mixer::Rule> rules;
QJsonValue mixerRulesJson = mixerJson.toObject().value("rules");
QJsonArray mixerRulesJsonArr = mixerRulesJson.toArray();
for (const auto& mixerRuleJson : mixerRulesJsonArr) {
QJsonValue mixerRule = mixerRuleJson.toObject();
Mixer::Rule rule{};
rule.selectIdentifier = mixerRule["select-identifier"].toString();

QJsonArray identifiersJson = mixerRule["apply-identifiers"].toArray();
for (const auto& identifierJson : identifiersJson) {
rule.applyIdentifiers.append(identifierJson.toString());
}

QJsonObject itemsJson = mixerRule["items"].toObject();
for (const auto& itemKey : itemsJson.keys()) {
bool ok;
int key = itemKey.toInt(&ok);
if (ok) {
QJsonArray itemsArr = itemsJson.value(itemKey).toArray();
QList<Mixer::Rule::RuleItem> items{};
for (const auto& itemJson : itemsArr) {
QJsonObject itemObj = itemJson.toObject();

Mixer::Rule::RuleItem item{};
if (itemObj.contains("min")) {
item.hasMin = true;
item.min = itemObj["min"].toDouble();
}
if (itemObj.contains("max")) {
item.hasMax = true;
item.max = itemObj["max"].toDouble();
}
if (itemObj.contains("default")) {
item.hasDefault = true;
item.defaultVal = itemObj["default"].toDouble();
}
item.hidden = itemObj["hidden"].toBool(false);
item.disabled = itemObj["disabled"].toBool(false);
items.append(item);
}
if (items.size() == rule.applyIdentifiers.size()) {
rule.items[key] = items;
} else {
qCWarning(ActuatorsConfigLog) << "Rules: unexpected num items in " << itemsArr << "expected:" << rule.applyIdentifiers.size();
}
}
}
rules.append(rule);
}

_mixer.reset(actuatorTypes, mixerOptions, outputFunctions, rules);
_init = true;
return true;
}
Expand Down
118 changes: 109 additions & 9 deletions src/Vehicle/Actuators/Mixer.cc
View file
Expand Up @@ -16,8 +16,9 @@
using namespace Mixer;

MixerChannel::MixerChannel(QObject *parent, const QString &label, int actuatorFunction, int paramIndex, int actuatorTypeIndex,
QmlObjectListModel &channelConfigs, ParameterManager* parameterManager, std::function<void(Fact*)> factAddedCb) :
QObject(parent), _label(label), _actuatorFunction(actuatorFunction), _paramIndex(paramIndex), _actuatorTypeIndex(actuatorTypeIndex)
QmlObjectListModel &channelConfigs, ParameterManager* parameterManager, const Rule* rule, std::function<void(Fact*)> factAddedCb) :
QObject(parent), _label(label), _actuatorFunction(actuatorFunction), _paramIndex(paramIndex), _actuatorTypeIndex(actuatorTypeIndex),
_rule(rule)
{
for (int i = 0; i < channelConfigs.count(); ++i) {
auto channelConfig = channelConfigs.value<ChannelConfig*>(i);
Expand Down Expand Up @@ -57,9 +58,85 @@ MixerChannel::MixerChannel(QObject *parent, const QString &label, int actuatorFu
qCDebug(ActuatorsConfigLog) << "ActuatorOutputChannel: Param does not exist:" << param;
}

ChannelConfigInstance* instance = new ChannelConfigInstance(this, fact, *channelConfig);
// if we have a valid rule, check the identifiers
int applyIdentifierIdx = -1;
if (rule) {
if (channelConfig->identifier() == rule->selectIdentifier) {
_ruleSelectIdentifierIdx = _configInstances->count();
if (fact) {
_currentSelectIdentifierValue = fact->rawValue().toInt();
}
} else {
for (int i = 0; i < rule->applyIdentifiers.size(); ++i) {
if (channelConfig->identifier() == rule->applyIdentifiers[i]) {
applyIdentifierIdx = i;
}
}
}

if (fact) {
connect(fact, &Fact::rawValueChanged, this, [this]() { applyRule(); });
}
}

ChannelConfigInstance* instance = new ChannelConfigInstance(this, fact, *channelConfig, applyIdentifierIdx);
_configInstances->append(instance);
}

applyRule(true);
}

void MixerChannel::applyRule(bool noConstraints)
{
if (!_rule || _ruleSelectIdentifierIdx == -1 || _applyingRule) {
return;
}
_applyingRule = true; // prevent recursive signals

Fact* selectFact = _configInstances->value<ChannelConfigInstance*>(_ruleSelectIdentifierIdx)->fact();
if (selectFact && selectFact->type() == FactMetaData::valueTypeInt32) {
const int value = selectFact->rawValue().toInt();
if (_rule->items.contains(value)) {
bool valueChanged = value != _currentSelectIdentifierValue;
const auto& items = _rule->items[value];
for (int i = 0; i < _configInstances->count(); ++i) {
ChannelConfigInstance* configInstance = _configInstances->value<ChannelConfigInstance*>(i);
if (configInstance->fact() && configInstance->ruleApplyIdentifierIdx() >= 0) {
const Rule::RuleItem& ruleItem = items[configInstance->ruleApplyIdentifierIdx()];
double factValue = configInstance->fact()->rawValue().toDouble();
bool changed = false;
if (ruleItem.hasMin && factValue < ruleItem.min) {
factValue = ruleItem.min;
changed = true;
}
if (ruleItem.hasMax && factValue > ruleItem.max) {
factValue = ruleItem.max;
changed = true;
}
if (valueChanged && ruleItem.hasDefault) {
factValue = ruleItem.defaultVal;
changed = true;
}
if (changed && !noConstraints) {
// here we could notify the user that a constraint got applied...
configInstance->fact()->setRawValue(factValue);
}
configInstance->setVisible(!ruleItem.hidden);
configInstance->setEnabled(!ruleItem.disabled);
}
}

} else {
// no rule set for this value, just reset
for (int i = 0; i < _configInstances->count(); ++i) {
ChannelConfigInstance* configInstance = _configInstances->value<ChannelConfigInstance*>(i);
configInstance->setVisible(true);
configInstance->setEnabled(true);
}
}
_currentSelectIdentifierValue = value;
}
_applyingRule = false;
}

bool MixerChannel::getGeometry(const ActuatorTypes& actuatorTypes, const MixerOption::ActuatorGroup& group,
Expand Down Expand Up @@ -126,13 +203,14 @@ void MixerConfigGroup::addConfigParam(ConfigParameter* param)
}

void Mixers::reset(const ActuatorTypes& actuatorTypes, const MixerOptions& mixerOptions,
const QMap<int, OutputFunction>& functions)
const QMap<int, OutputFunction>& functions, const Rules& rules)
{
_groups->clearAndDeleteContents();
_actuatorTypes = actuatorTypes;
_mixerOptions = mixerOptions;
_functions = functions;
_functionsSpecificLabel.clear();
_rules = rules;
_mixerConditions.clear();
for (const auto& mixerOption : _mixerOptions) {
_mixerConditions.append(Condition(mixerOption.option, _parameterManager));
Expand Down Expand Up @@ -188,8 +266,18 @@ void Mixers::update()
currentMixerGroup->addChannelConfig(new ChannelConfig(currentMixerGroup, param, true));
}
}

const Rule* selectedRule{nullptr}; // at most 1 rule can be applied
for (const auto& perItemParam : actuatorGroup.perItemParameters) {
currentMixerGroup->addChannelConfig(new ChannelConfig(currentMixerGroup, perItemParam, false));

if (!perItemParam.identifier.isEmpty()) {
for (const auto& rule : _rules) {
if (rule.selectIdentifier == perItemParam.identifier) {
selectedRule = &rule;
}
}
}
}

// 'count' param
Expand Down Expand Up @@ -221,7 +309,7 @@ void Mixers::update()
}
}
MixerChannel* channel = new MixerChannel(currentMixerGroup, label, actuatorFunction, actuatorIdx, actuatorTypeIndex,
*currentMixerGroup->channelConfigs(), _parameterManager, [this](Fact* fact) { subscribeFact(fact); });
*currentMixerGroup->channelConfigs(), _parameterManager, selectedRule, [this](Fact* fact) { subscribeFact(fact, true); });
currentMixerGroup->addChannel(channel);
++actuatorTypeIndex;
}
Expand Down Expand Up @@ -287,11 +375,18 @@ Fact* Mixers::getFact(const QString& paramName)
return fact;
}

void Mixers::subscribeFact(Fact* fact)
void Mixers::subscribeFact(Fact* fact, bool geometry)
{
if (fact && !_subscribedFacts.contains(fact)) {
connect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
_subscribedFacts.insert(fact);
if (geometry) {
if (fact && !_subscribedFactsGeometry.contains(fact)) {
connect(fact, &Fact::rawValueChanged, this, &Mixers::geometryParamChanged);
_subscribedFactsGeometry.insert(fact);
}
} else {
if (fact && !_subscribedFacts.contains(fact)) {
connect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
_subscribedFacts.insert(fact);
}
}
}

Expand All @@ -301,4 +396,9 @@ void Mixers::unsubscribeFacts()
disconnect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
}
_subscribedFacts.clear();

for (Fact* fact : _subscribedFactsGeometry) {
disconnect(fact, &Fact::rawValueChanged, this, &Mixers::geometryParamChanged);
}
_subscribedFactsGeometry.clear();
}

0 comments on commit fdd141e

Please sign in to comment.