diff --git a/_codeql_detected_source_root b/_codeql_detected_source_root new file mode 120000 index 00000000..945c9b46 --- /dev/null +++ b/_codeql_detected_source_root @@ -0,0 +1 @@ +. \ No newline at end of file diff --git a/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp new file mode 100644 index 00000000..7f1c0285 --- /dev/null +++ b/include/arbitration_graphs/internal/sequence_arbitrator_io.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include "../sequence_arbitrator.hpp" + + +namespace arbitration_graphs { + + +/////////////////////////////////////// +// SequenceArbitrator::Option // +/////////////////////////////////////// + +template +std::ostream& SequenceArbitrator::Option::toStream( + std::ostream& output, + const Time& time, + const EnvironmentModelT& environmentModel, + const int& optionIndex, + const std::string& prefix, + const std::string& suffix) const { + output << optionIndex + 1 << ". "; + ArbitratorBase::Option::toStream(output, time, environmentModel, optionIndex, prefix, suffix); + return output; +} + + +/////////////////////////////////////// +// SequenceArbitrator // +/////////////////////////////////////// + +template +std::ostream& SequenceArbitrator::toStream( + std::ostream& output, + const Time& time, + const EnvironmentModelT& environmentModel, + const std::string& prefix, + const std::string& suffix) const { + + Behavior::toStream(output, time, environmentModel, prefix, suffix); + + for (int i = 0; i < static_cast(this->options().size()); ++i) { + const typename ArbitratorBase::Option::ConstPtr option = this->options().at(i); + const bool isCurrent = sequenceStarted_ && currentBehaviorActivated_ && (i == currentIndex_); + + if (isCurrent) { + output << suffix << '\n' << prefix << " -> "; + } else { + output << suffix << '\n' << prefix << " "; + } + option->toStream(output, time, environmentModel, i, " " + prefix, suffix); + } + return output; +} + +template +YAML::Node SequenceArbitrator::toYaml( + const Time& time, const EnvironmentModelT& environmentModel) const { + YAML::Node node = Behavior::toYaml(time, environmentModel); + + node["type"] = "SequenceArbitrator"; + for (const typename ArbitratorBase::Option::ConstPtr& option : this->options()) { + node["options"].push_back(option->toYaml(time, environmentModel)); + } + if (sequenceStarted_ && currentBehaviorActivated_) { + node["activeBehavior"] = currentIndex_; + } + + return node; +} + +} // namespace arbitration_graphs diff --git a/include/arbitration_graphs/sequence_arbitrator.hpp b/include/arbitration_graphs/sequence_arbitrator.hpp new file mode 100644 index 00000000..fce3e27b --- /dev/null +++ b/include/arbitration_graphs/sequence_arbitrator.hpp @@ -0,0 +1,213 @@ +#pragma once + +#include + +#include + +#include "arbitrator.hpp" +#include "exceptions.hpp" + + +namespace arbitration_graphs { + +/*! + * \brief A sequence arbitrator that executes its sub-behaviors in a fixed order. + * + * Based on the definition by Lauer et al. (2010): + * - Invocation condition: The invocation condition of the first sub-behavior is true. + * - Commitment condition: The sequence has started and has not yet completed. + * - Algorithm: Executes sub-behaviors in the order they were added. Advances to the next + * sub-behavior once the current sub-behavior's commitment condition becomes false. + * The sequence is complete when the last sub-behavior's commitment condition + * becomes false. + * + * \see Martin Lauer, Roland Hafner, Sascha Lange, and Martin Riedmiller, + * "Cognitive concepts in autonomous soccer playing robots," + * Cognitive Systems Research, vol. 11, no. 3, pp. 287–309, 2010, + * doi: https://doi.org/10.1016/j.cogsys.2009.12.003 + */ +template +class SequenceArbitrator : public Arbitrator { +public: + using ArbitratorBase = Arbitrator; + + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + using PlaceboVerifierT = verification::PlaceboVerifier; + using VerifierT = verification::Verifier; + + class Option : public ArbitratorBase::Option { + public: + using Ptr = std::shared_ptr