From b27341b8532312cb87c9268872ff4bfe4ec95f9c Mon Sep 17 00:00:00 2001
From: Nick Le Large <nick.lelarge@kit.edu>
Date: Mon, 7 Oct 2024 15:27:40 +0200
Subject: [PATCH] Wrap getAndVerifyCommand in try catch block

Includes a basic unit test
---
 .../internal/arbitrator_impl.hpp              | 12 ++++-
 test/dummy_types.hpp                          | 12 ++++-
 test/handle_exceptions.cpp                    | 51 +++++++++++++++++++
 3 files changed, 72 insertions(+), 3 deletions(-)
 create mode 100644 test/handle_exceptions.cpp

diff --git a/include/arbitration_graphs/internal/arbitrator_impl.hpp b/include/arbitration_graphs/internal/arbitrator_impl.hpp
index 8e7010ca..955a514c 100644
--- a/include/arbitration_graphs/internal/arbitrator_impl.hpp
+++ b/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_.reset();
+            bestOption->behavior_->loseControl(time);
+            continue;
+        }
 
         if (command) {
             if (activeBehavior_ && bestOption != activeBehavior_) {
@@ -127,4 +135,4 @@ SubCommandT Arbitrator<CommandT, SubCommandT, VerifierT, VerificationResultT>::g
                                                     " applicable options passed the verification step!");
 }
 
-} // namespace arbitration_graphs
\ No newline at end of file
+} // namespace arbitration_graphs
diff --git a/test/dummy_types.hpp b/test/dummy_types.hpp
index d0304d46..f2de851c 100644
--- a/test/dummy_types.hpp
+++ b/test/dummy_types.hpp
@@ -62,6 +62,16 @@ class DummyBehavior : public Behavior<DummyCommand> {
     int loseControlCounter_;
 };
 
+class BrokenDummyBehavior : public DummyBehavior {
+public:
+    BrokenDummyBehavior(const bool invocation, const bool commitment, const std::string& name = "BrokenDummyBehavior")
+            : DummyBehavior(invocation, commitment, name) {};
+
+    DummyCommand getCommand(const Time& time) override {
+        throw std::runtime_error("BrokenDummyBehavior::getCommand() is broken");
+    }
+};
+
 struct DummyResult {
     bool isOk() const {
         return isOk_;
@@ -76,4 +86,4 @@ struct DummyResult {
 inline std::ostream& operator<<(std::ostream& out, const arbitration_graphs_tests::DummyResult& result) {
     out << (result.isOk() ? "is okay" : "is not okay");
     return out;
-}
\ No newline at end of file
+}
diff --git a/test/handle_exceptions.cpp b/test/handle_exceptions.cpp
new file mode 100644
index 00000000..2462f838
--- /dev/null
+++ b/test/handle_exceptions.cpp
@@ -0,0 +1,51 @@
+#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));
+    EXPECT_FALSE(testPriorityArbitrator.options().at(0)->verificationResult_.cached(time));
+    ASSERT_TRUE(testPriorityArbitrator.options().at(1)->verificationResult_.cached(time));
+
+    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);
+}
+