diff --git a/include/arbitration_graphs/internal/arbitrator_impl.hpp b/include/arbitration_graphs/internal/arbitrator_impl.hpp index 5ec6f5ac..deb73023 100644 --- a/include/arbitration_graphs/internal/arbitrator_impl.hpp +++ b/include/arbitration_graphs/internal/arbitrator_impl.hpp @@ -110,7 +110,15 @@ SubCommandT Arbitrator::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 command = getAndVerifyCommand(bestOption, time); + std::optional 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_) { diff --git a/include/arbitration_graphs/verification.hpp b/include/arbitration_graphs/verification.hpp index 9a1316f5..b0a2a8c7 100644 --- a/include/arbitration_graphs/verification.hpp +++ b/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 struct PlaceboVerifier { diff --git a/test/dummy_types.hpp b/test/dummy_types.hpp index 37126a5e..9fb5dbfd 100644 --- a/test/dummy_types.hpp +++ b/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 { 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 diff --git a/test/handle_exceptions.cpp b/test/handle_exceptions.cpp new file mode 100644 index 00000000..10afa598 --- /dev/null +++ b/test/handle_exceptions.cpp @@ -0,0 +1,52 @@ +#include +#include +#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(true, true, "HighPriority"); + DummyBehavior::Ptr testBehaviorLowPriority = std::make_shared(true, true, "LowPriority"); + + Time time{Clock::now()}; +}; + +TEST_F(ExceptionHandlingTest, HandleException) { + using OptionFlags = PriorityArbitrator::Option::Flags; + + PriorityArbitrator 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); +} +