Add SequenceArbitrator

_codeql_detected_source_root

@@ -0,0 +1 @@
+.

include/arbitration_graphs/internal/sequence_arbitrator_io.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include "../sequence_arbitrator.hpp"
+
+
+namespace arbitration_graphs {
+
+
+///////////////////////////////////////
+//    SequenceArbitrator::Option    //
+///////////////////////////////////////
+
+template <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+std::ostream& SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::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 <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+std::ostream& SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toStream(
+    std::ostream& output,
+    const Time& time,
+    const EnvironmentModelT& environmentModel,
+    const std::string& prefix,
+    const std::string& suffix) const {
+
+    Behavior<EnvironmentModelT, CommandT>::toStream(output, time, environmentModel, prefix, suffix);
+
+    for (int i = 0; i < static_cast<int>(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 <typename EnvironmentModelT, typename CommandT, typename SubCommandT>
+YAML::Node SequenceArbitrator<EnvironmentModelT, CommandT, SubCommandT>::toYaml(
+    const Time& time, const EnvironmentModelT& environmentModel) const {
+    YAML::Node node = Behavior<EnvironmentModelT, CommandT>::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

include/arbitration_graphs/sequence_arbitrator.hpp

@@ -0,0 +1,213 @@
+#pragma once
+
+#include <memory>
+
+#include <yaml-cpp/yaml.h>
+
+#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 <typename EnvironmentModelT, typename CommandT, typename SubCommandT = CommandT>
+class SequenceArbitrator : public Arbitrator<EnvironmentModelT, CommandT, SubCommandT> {
+public:
+    using ArbitratorBase = Arbitrator<EnvironmentModelT, CommandT, SubCommandT>;
+
+    using Ptr = std::shared_ptr<SequenceArbitrator>;
+    using ConstPtr = std::shared_ptr<const SequenceArbitrator>;
+
+    using PlaceboVerifierT = verification::PlaceboVerifier<EnvironmentModelT, SubCommandT>;
+    using VerifierT = verification::Verifier<EnvironmentModelT, SubCommandT>;
+
+    class Option : public ArbitratorBase::Option {
+    public:
+        using Ptr = std::shared_ptr<Option>;
+        using FlagsT = typename ArbitratorBase::Option::FlagsT;
+        using ConstPtr = std::shared_ptr<const Option>;
+
+        enum Flags { NoFlags = 0b0 };
+
+        Option(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior, const FlagsT& flags)
+                : ArbitratorBase::Option(behavior, flags) {
+        }
+
+        /*!
+         * \brief Writes a string representation of the behavior option and its current state to the output stream.
+         *
+         * \param output            Output stream to write into, will be returned also
+         * \param time              Expected execution time point of this behaviors command
+         * \param environmentModel  A read-only object containing the current state of the environment
+         * \param optionIndex       Position index of this option within options()
+         * \param prefix            A string that should be prepended to each line that is written to the output stream
+         * \param suffix            A string that should be appended to each line that is written to the output stream
+         * \return                  The same given input stream (signature similar to std::ostream& operator<<())
+         *
+         * \see Arbitrator::toStream()
+         */
+        std::ostream& toStream(std::ostream& output,
+                               const Time& time,
+                               const EnvironmentModelT& environmentModel,
+                               const int& optionIndex,
+                               const std::string& prefix = "",
+                               const std::string& suffix = "") const override;
+    };
+
+    explicit SequenceArbitrator(const std::string& name = "SequenceArbitrator",
+                                typename VerifierT::Ptr verifier = std::make_shared<PlaceboVerifierT>())
+            : ArbitratorBase(name, verifier) {};
+
+    void addOption(const typename Behavior<EnvironmentModelT, SubCommandT>::Ptr& behavior,
+                   const typename Option::FlagsT& flags) override {
+        typename Option::Ptr option = std::make_shared<Option>(behavior, flags);
+        this->addOptionImpl(option);
+        sequenceOptions_.push_back(option);
+    }
+
+    /*!
+     * \brief The invocation condition is true if the first sub-behavior's invocation condition is true.
+     */
+    bool checkInvocationCondition(const Time& time, const EnvironmentModelT& environmentModel) const override {
+        if (sequenceOptions_.empty()) {
+            return false;
+        }
+        return sequenceOptions_.front()->behavior()->checkInvocationCondition(time, environmentModel);
+    }
+
+    /*!
+     * \brief The commitment condition is true as long as the sequence has started and has not yet completed.
+     *
+     * The sequence is committed if the current sub-behavior's commitment condition is true,
+     * or if there are more sub-behaviors to execute after the current one.
+     */
+    bool checkCommitmentCondition(const Time& time, const EnvironmentModelT& environmentModel) const override {
+        if (!sequenceStarted_ || sequenceOptions_.empty()) {
+            return false;
+        }
+        return sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel) ||
+               currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size());
+    }
+
+    /*!
+     * \brief Starts execution of the sequence, resetting to the first sub-behavior.
+     *
+     * The first sub-behavior will be activated lazily on the first call to getCommand().
+     */
+    void gainControl(const Time& /*time*/, const EnvironmentModelT& /*environmentModel*/) override {
+        currentIndex_ = 0;
+        sequenceStarted_ = true;
+        currentBehaviorActivated_ = false;
+    }
+
+    /*!
+     * \brief Stops execution of the sequence and cleans up the currently active sub-behavior.
+     */
+    void loseControl(const Time& time, const EnvironmentModelT& environmentModel) override {
+        if (sequenceStarted_ && currentBehaviorActivated_ && !sequenceOptions_.empty()) {
+            sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
+        }
+        currentIndex_ = 0;
+        sequenceStarted_ = false;
+        currentBehaviorActivated_ = false;
+    }
+
+    /*!
+     * \brief Executes sub-behaviors in sequence, advancing by one step when the current one is done.
+     *
+     * On each call, the current sub-behavior is activated (if not already) and its command is returned.
+     * If the current sub-behavior's commitment condition is false on the next call and a subsequent
+     * sub-behavior exists, the sequence advances exactly one step: loseControl on the current,
+     * gainControl on the next.
+     */
+    CommandT getCommand(const Time& time, const EnvironmentModelT& environmentModel) override {
+        if (!sequenceStarted_ || sequenceOptions_.empty()) {
+            throw InvocationConditionIsFalseError(
+                "SequenceArbitrator::getCommand() called without prior gainControl() or with no options!");
+        }
+
+        // If the current sub-behavior has been executed before and its CC is now false, advance one step
+        if (currentBehaviorActivated_ &&
+            currentIndex_ + 1 < static_cast<int>(sequenceOptions_.size()) &&
+            !sequenceOptions_.at(currentIndex_)->behavior()->checkCommitmentCondition(time, environmentModel)) {
+            sequenceOptions_.at(currentIndex_)->behavior()->loseControl(time, environmentModel);
+            currentIndex_++;
+            currentBehaviorActivated_ = false;
+        }
+
+        // Lazily activate the current sub-behavior on its first execution
+        if (!currentBehaviorActivated_) {
+            sequenceOptions_.at(currentIndex_)->behavior()->gainControl(time, environmentModel);
+            currentBehaviorActivated_ = true;
+        }
+
+        // Get and verify command from the current sub-behavior
+        typename ArbitratorBase::Option::Ptr currentOption = sequenceOptions_.at(currentIndex_);
+        std::optional<SubCommandT> command = this->getAndVerifyCommand(currentOption, time, environmentModel);
+        if (command) {
+            return command.value();
+        }
+
+        throw NoApplicableOptionPassedVerificationError(
+            "Current sub-behavior in sequence failed verification!");
+    }
+
+    bool isActive() const override {
+        return sequenceStarted_;
+    }
+
+    /*!
+     * \brief Writes a string representation of the SequenceArbitrator with its current state to the output stream.
+     */
+    std::ostream& toStream(std::ostream& output,
+                           const Time& time,
+                           const EnvironmentModelT& environmentModel,
+                           const std::string& prefix = "",
+                           const std::string& suffix = "") const override;
+
+    /*!
+     * \brief Returns a yaml representation of the arbitrator object with its current state
+     *
+     * \param time  Expected execution time point of this behaviors command
+     * \param environmentModel  A read-only object containing the current state of the environment
+     * \return      Yaml representation of this behavior
+     */
+    YAML::Node toYaml(const Time& time, const EnvironmentModelT& environmentModel) const override;
+
+protected:
+    /*!
+     * \brief Not used by SequenceArbitrator (getCommand is fully overridden), but required as pure virtual.
+     */
+    typename ArbitratorBase::Options sortOptionsByGivenPolicy(
+        const typename ArbitratorBase::Options& options,
+        const Time& /*time*/,
+        const EnvironmentModelT& /*environmentModel*/) const override {
+        return options;
+    }
+
+private:
+    std::vector<typename Option::Ptr> sequenceOptions_;
+    int currentIndex_{0};
+    bool sequenceStarted_{false};
+    bool currentBehaviorActivated_{false};
+};
+
+} // namespace arbitration_graphs
+
+#include "internal/sequence_arbitrator_io.hpp" // IWYU pragma: keep

python_bindings/include/arbitration_graphs_py/sequence_arbitrator.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <pybind11/pybind11.h>
+#include <yaml-cpp/yaml.h>
+
+#include <arbitration_graphs/arbitrator.hpp>
+#include <arbitration_graphs/behavior.hpp>
+#include <arbitration_graphs/sequence_arbitrator.hpp>
+
+#include "command_wrapper.hpp"
+#include "environment_model_wrapper.hpp"
+#include "verification.hpp"
+#include "yaml_helper.hpp"
+
+namespace arbitration_graphs_py {
+
+namespace py = pybind11;
+namespace ag = arbitration_graphs;
+
+inline void bindSequenceArbitrator(py::module& module) {
+    using Time = ag::Time;
+
+    using ArbitratorT = ag::Arbitrator<EnvironmentModelWrapper, CommandWrapper>;
+    using ArbitratorOptionT = typename ArbitratorT::Option;
+
+    using BehaviorT = typename ArbitratorT::Behavior;
+
+    using SequenceArbitratorT = ag::SequenceArbitrator<EnvironmentModelWrapper, CommandWrapper>;
+
+    using OptionT = typename SequenceArbitratorT::Option;
+    using FlagsT = typename OptionT::FlagsT;
+
+    using VerifierT = ag::verification::Verifier<EnvironmentModelWrapper, CommandWrapper>;
+    using PlaceboVerifierT = ag::verification::PlaceboVerifier<EnvironmentModelWrapper, CommandWrapper>;
+
+    py::classh<SequenceArbitratorT, ArbitratorT> sequenceArbitrator(module, "SequenceArbitrator");
+    sequenceArbitrator
+        .def(py::init<const std::string&, const VerifierT::Ptr&>(),
+             py::arg("name") = "SequenceArbitrator",
+             py::arg("verifier") = PlaceboVerifierT())
+        .def("add_option", &SequenceArbitratorT::addOption, py::arg("behavior"), py::arg("flags"))
+        .def(
+            "to_yaml",
+            [](const SequenceArbitratorT& self, const Time& time, const EnvironmentModelWrapper& environmentModel) {
+                return yaml_helper::toYamlAsPythonObject(self, time, environmentModel);
+            },
+            py::arg("time"),
+            py::arg("environment_model"))
+        .def("__repr__",
+             [](const SequenceArbitratorT& self) { return "<SequenceArbitrator '" + self.name() + "'>"; });
+
+    py::classh<OptionT, ArbitratorOptionT> option(sequenceArbitrator, "Option");
+    option.def(py::init<const typename BehaviorT::Ptr&, const FlagsT&>(), py::arg("behavior"), py::arg("flags"));
+
+    py::enum_<typename OptionT::Flags>(option, "Flags").value("NO_FLAGS", OptionT::NoFlags);
+}
+
+} // namespace arbitration_graphs_py

python_bindings/src/bindings.cpp

@@ -8,6 +8,7 @@
 #include "arbitration_graphs_py/exceptions.hpp"
 #include "arbitration_graphs_py/priority_arbitrator.hpp"
 #include "arbitration_graphs_py/random_arbitrator.hpp"
+#include "arbitration_graphs_py/sequence_arbitrator.hpp"
 #include "arbitration_graphs_py/verifier.hpp"
 
 #define STRINGIFY(x) #x
@@ -28,6 +29,7 @@ PYBIND11_MODULE(arbitration_graphs_py, mainModule) {
     bindCostArbitrator(mainModule);
     bindPriorityArbitrator(mainModule);
     bindRandomArbitrator(mainModule);
+    bindSequenceArbitrator(mainModule);
 
     // Add the __version__ attribute to the module
 #ifdef PROJECT_VERSION