From 55589042480396aa5660a6a748faf7d58f1fbb8c Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Mon, 18 Dec 2023 11:20:24 +0100 Subject: [PATCH] Timeout in BehaviorKit --- include/interface/libs/BehaviorKit.h | 2 ++ libs/BehaviorKit/include/BehaviorKit.h | 6 +++++- libs/BehaviorKit/source/BehaviorKit.cpp | 13 ++++++++++++- libs/BehaviorKit/tests/BehaviorKit_test.cpp | 17 +++++++++++++++-- spikes/lk_auto_charge/main.cpp | 2 +- 5 files changed, 35 insertions(+), 5 deletions(-) diff --git a/include/interface/libs/BehaviorKit.h b/include/interface/libs/BehaviorKit.h index 50755131b4..e5181647f3 100644 --- a/include/interface/libs/BehaviorKit.h +++ b/include/interface/libs/BehaviorKit.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include "interface/Behavior.h" @@ -17,6 +18,7 @@ class BehaviorKit virtual ~BehaviorKit() = default; virtual void registerBehaviors(std::span behaviors) = 0; + virtual void setTimeoutDuration(std::chrono::seconds duration) = 0; virtual void start(interface::Behavior *behavior) = 0; virtual void start(BehaviorID id) = 0; virtual void stop() = 0; diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKit/include/BehaviorKit.h index 30be72fe96..a89807735b 100644 --- a/libs/BehaviorKit/include/BehaviorKit.h +++ b/libs/BehaviorKit/include/BehaviorKit.h @@ -7,6 +7,7 @@ #include #include "interface/Behavior.h" +#include "interface/drivers/Timeout.h" #include "interface/libs/BehaviorKit.h" #include "interface/libs/EventLoop.h" #include "internal/BehaviorID.h" @@ -16,9 +17,10 @@ namespace leka { class BehaviorKit : public interface::BehaviorKit { public: - explicit BehaviorKit(interface::EventLoop &event_loop); + explicit BehaviorKit(interface::EventLoop &event_loop, interface::Timeout &timeout); void registerBehaviors(std::span behaviors) final; + void setTimeoutDuration(std::chrono::seconds duration) final; void start(interface::Behavior *behavior) final; void start(BehaviorID id) final; @@ -28,6 +30,8 @@ class BehaviorKit : public interface::BehaviorKit void run(); interface::EventLoop &_event_loop; + interface::Timeout &_timeout; + std::chrono::seconds _timeout_duration {20}; std::span _behaviors {}; interface::Behavior *_behavior = nullptr; diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKit/source/BehaviorKit.cpp index c1c943659b..2dc84ffeff 100644 --- a/libs/BehaviorKit/source/BehaviorKit.cpp +++ b/libs/BehaviorKit/source/BehaviorKit.cpp @@ -7,9 +7,11 @@ using namespace leka; -BehaviorKit::BehaviorKit(interface::EventLoop &event_loop) : _event_loop(event_loop) +BehaviorKit::BehaviorKit(interface::EventLoop &event_loop, interface::Timeout &timeout) + : _event_loop(event_loop), _timeout(timeout) { _event_loop.registerCallback([this] { run(); }); + _timeout.onTimeout([this] { stop(); }); } void BehaviorKit::registerBehaviors(std::span behaviors) @@ -17,6 +19,11 @@ void BehaviorKit::registerBehaviors(std::span behaviors) _behaviors = behaviors; } +void BehaviorKit::setTimeoutDuration(std::chrono::seconds duration) +{ + _timeout_duration = duration; +} + void BehaviorKit::start(interface::Behavior *behavior) { stop(); @@ -28,6 +35,9 @@ void BehaviorKit::start(interface::Behavior *behavior) } _event_loop.start(); + if (_timeout_duration != std::chrono::seconds {0}) { + _timeout.start(_timeout_duration); + } } void BehaviorKit::start(BehaviorID id) @@ -54,4 +64,5 @@ void BehaviorKit::stop() if (_behavior != nullptr) { _behavior->stop(); } + _timeout.stop(); } diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKit/tests/BehaviorKit_test.cpp index 34fadde6bc..7b6b16f1ab 100644 --- a/libs/BehaviorKit/tests/BehaviorKit_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorKit_test.cpp @@ -7,6 +7,7 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" #include "mocks/leka/Behavior.h" +#include "mocks/leka/Timeout.h" #include "stubs/leka/EventLoopKit.h" using namespace leka; @@ -22,7 +23,8 @@ class BehaviorKitTest : public ::testing::Test // void TearDown() override {} stub::EventLoopKit stub_event_loop {}; - BehaviorKit behaviorkit {stub_event_loop}; + mock::Timeout mock_timeout {}; + BehaviorKit behaviorkit {stub_event_loop, mock_timeout}; mock::Behavior mock_behavior_a {}; mock::Behavior mock_behavior_b {}; @@ -38,6 +40,8 @@ TEST_F(BehaviorKitTest, startFirstBehavior) auto behaviors = std::to_array({&mock_behavior_a}); behaviorkit.registerBehaviors(behaviors); + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start); EXPECT_CALL(mock_behavior_a, run); behaviorkit.start(&mock_behavior_a); @@ -45,7 +49,8 @@ TEST_F(BehaviorKitTest, startFirstBehavior) TEST_F(BehaviorKitTest, startBehaviorNullPtr) { - // nothing expected + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start).Times(0); behaviorkit.start(nullptr); } @@ -61,6 +66,8 @@ TEST_F(BehaviorKitTest, startFirstBehaviorID) auto behavior_a_id = BehaviorID {1}; auto behavior_b_id = BehaviorID {2}; + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start); EXPECT_CALL(mock_behavior_a, id).WillRepeatedly(Return(behavior_a_id)); EXPECT_CALL(mock_behavior_b, id).WillRepeatedly(Return(behavior_b_id)); EXPECT_CALL(mock_behavior_b, run); @@ -79,6 +86,8 @@ TEST_F(BehaviorKitTest, startBehaviorIDNotRegistered) auto behavior_a_id = BehaviorID {1}; auto behavior_b_id = BehaviorID {2}; + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start).Times(0); EXPECT_CALL(mock_behavior_a, id).WillRepeatedly(Return(behavior_a_id)); EXPECT_CALL(mock_behavior_b, id).WillRepeatedly(Return(behavior_b_id)); @@ -93,10 +102,14 @@ TEST_F(BehaviorKitTest, startAnyBehavior) }); behaviorkit.registerBehaviors(behaviors); + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start); EXPECT_CALL(mock_behavior_a, run); behaviorkit.start(&mock_behavior_a); + EXPECT_CALL(mock_timeout, stop); + EXPECT_CALL(mock_timeout, start); EXPECT_CALL(mock_behavior_a, stop); EXPECT_CALL(mock_behavior_b, run); diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index 812d75a537..0ab60d4cc8 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -104,7 +104,7 @@ auto blekit = BLEKit {}; auto behavior_autocharge_seal = behavior::AutochargeSeal {motors::left::motor, motors::right::motor, imukit}; auto behaviors = std::to_array({&behavior_autocharge_seal}); -auto behavior_kit = BehaviorKit {event_loop}; +auto behavior_kit = BehaviorKit {event_loop, timeout}; auto last_strategy = BehaviorID {0x00};