Wrap getAndVerifyCommand in try catch block

include/arbitration_graphs/internal/arbitrator_impl.hpp

@@ -110,7 +110,15 @@ SubCommandT Arbitrator<CommandT, SubCommandT, VerifierT, VerificationResultT>::g
         // otherwise we have bestOption == activeBehavior_ which already gained control
 
         // an arbitrator as option might not return a command, if its applicable options fail verification:
-        const std::optional<SubCommandT> command = getAndVerifyCommand(bestOption, time);
+        std::optional<SubCommandT> command;
+        try {
+            command = getAndVerifyCommand(bestOption, time);
+        } catch (const std::exception& e) {
+            VLOG(1) << bestOption->behavior_->name_ << " threw an exception during getAndVerifyCommand(): " << e.what();
+            bestOption->verificationResult_.cache(time, VerificationResultT{false});
+            bestOption->behavior_->loseControl(time);
+            continue;
+        }
 
         if (command) {
             if (activeBehavior_ && bestOption != activeBehavior_) {

include/arbitration_graphs/verification.hpp

@@ -14,9 +14,11 @@ namespace arbitration_graphs::verification {
  * robots. Otherwise these are a good starting point to implement your own meaningful verifier.
  */
 struct PlaceboResult {
-    static bool isOk() {
-        return true;
+    bool isOk() const {
+        return isOk_;
     };
+
+    bool isOk_{true};
 };
 template <typename DataT>
 struct PlaceboVerifier {

test/dummy_types.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "behavior.hpp"
+#include "verification.hpp"
 
 
 namespace arbitration_graphs_tests {
@@ -63,14 +64,18 @@ class DummyBehavior : public Behavior<DummyCommand> {
     int loseControlCounter_{0};
 };
 
-struct DummyResult {
-    bool isOk() const {
-        return isOk_;
-    };
+class BrokenDummyBehavior : public DummyBehavior {
+public:
+    BrokenDummyBehavior(const bool invocation, const bool commitment, const std::string& name = "BrokenDummyBehavior")
+            : DummyBehavior(invocation, commitment, name){};
 
-    bool isOk_;
+    DummyCommand getCommand(const Time& time) override {
+        throw std::runtime_error("BrokenDummyBehavior::getCommand() is broken");
+    }
 };
 
+struct DummyResult : public verification::PlaceboResult {};
+
 } // namespace arbitration_graphs_tests
 
 

test/handle_exceptions.cpp

@@ -0,0 +1,52 @@
+#include <optional>
+#include <string>
+#include "gtest/gtest.h"
+
+#include "behavior.hpp"
+#include "priority_arbitrator.hpp"
+
+#include "dummy_types.hpp"
+
+using namespace arbitration_graphs;
+using namespace arbitration_graphs_tests;
+
+class ExceptionHandlingTest : public ::testing::Test {
+protected:
+    BrokenDummyBehavior::Ptr testBehaviorHighPriority =
+        std::make_shared<BrokenDummyBehavior>(true, true, "HighPriority");
+    DummyBehavior::Ptr testBehaviorLowPriority = std::make_shared<DummyBehavior>(true, true, "LowPriority");
+
+    Time time{Clock::now()};
+};
+
+TEST_F(ExceptionHandlingTest, HandleException) {
+    using OptionFlags = PriorityArbitrator<DummyCommand>::Option::Flags;
+
+    PriorityArbitrator<DummyCommand> testPriorityArbitrator;
+
+    testPriorityArbitrator.addOption(testBehaviorHighPriority, OptionFlags::NO_FLAGS);
+    testPriorityArbitrator.addOption(testBehaviorLowPriority, OptionFlags::NO_FLAGS);
+
+    ASSERT_TRUE(testPriorityArbitrator.checkInvocationCondition(time));
+
+    testPriorityArbitrator.gainControl(time);
+
+    // Since the high priority behavior is broken, we should get the low priority behavior as fallback
+    EXPECT_EQ("LowPriority", testPriorityArbitrator.getCommand(time));
+    ASSERT_TRUE(testPriorityArbitrator.options().at(0)->verificationResult_.cached(time));
+    ASSERT_TRUE(testPriorityArbitrator.options().at(1)->verificationResult_.cached(time));
+
+    EXPECT_FALSE(testPriorityArbitrator.options().at(0)->verificationResult_.cached(time)->isOk());
+    EXPECT_TRUE(testPriorityArbitrator.options().at(1)->verificationResult_.cached(time)->isOk());
+
+    testPriorityArbitrator.loseControl(time);
+
+    testBehaviorLowPriority->invocationCondition_ = false;
+    ASSERT_TRUE(testPriorityArbitrator.checkInvocationCondition(time));
+
+    testPriorityArbitrator.gainControl(time);
+
+    // With no fallback, there is no option to call even if the invocation condition is true
+    EXPECT_THROW(testPriorityArbitrator.getCommand(time), NoApplicableOptionPassedVerificationError);
+}
+