From 974dd5c77315f44c8f3de3315daaea75aafd741f Mon Sep 17 00:00:00 2001 From: Jaci Brunning Date: Sun, 4 Dec 2022 21:08:11 +1100 Subject: [PATCH] Add Behaviour system --- BEHAVIOURS.md | 300 ++++++++++++ wombat/src/main/cpp/behaviour/Behaviour.cpp | 261 ++++++++++ .../main/cpp/behaviour/BehaviourScheduler.cpp | 73 +++ .../src/main/cpp/behaviour/HasBehaviour.cpp | 14 + wombat/src/main/include/behaviour/Behaviour.h | 455 ++++++++++++++++++ .../include/behaviour/BehaviourScheduler.h | 56 +++ .../src/main/include/behaviour/HasBehaviour.h | 34 ++ wombat/src/test/cpp/test_Behaviour.cpp | 366 ++++++++++++++ 8 files changed, 1559 insertions(+) create mode 100644 BEHAVIOURS.md create mode 100644 wombat/src/main/cpp/behaviour/Behaviour.cpp create mode 100644 wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp create mode 100644 wombat/src/main/cpp/behaviour/HasBehaviour.cpp create mode 100644 wombat/src/main/include/behaviour/Behaviour.h create mode 100644 wombat/src/main/include/behaviour/BehaviourScheduler.h create mode 100644 wombat/src/main/include/behaviour/HasBehaviour.h create mode 100644 wombat/src/test/cpp/test_Behaviour.cpp diff --git a/BEHAVIOURS.md b/BEHAVIOURS.md new file mode 100644 index 0000000..d788399 --- /dev/null +++ b/BEHAVIOURS.md @@ -0,0 +1,300 @@ +# Behaviours + +Behaviours are Curtin FRC's solution to the control problem. In a robot where many actions can occur at once, how do we make sure only one place has control of a system at a particular time? Moreover, how can we combine small elements of control into one much larger, cohesive functionality - such as chaining small autonomous actions together to make a full autonomous routine? + +A Behaviour is a small action or piece of work that describes how to control a robot system or collection of systems. Examples may include: +- Teleoperated control of the drivetrain +- Automatic spinup of the shooter +- Setting the elevator to go to a specific height +- Performing motor calibration +- ... and many more. + +The key to Behaviours is that they are *small*, and are used together to build into a much larger system. + +## Implementing Behaviours +Behaviours are created by inheriting from the `Behaviour` class. Let's look at an example Behaviour that prints a message when it starts, and then immediately stops. + +```cpp +// PrintBehaviour.h +#pragma once +#include + +class PrintBehaviour : public behaviour::Behaviour { + public: + PrintBehaviour(std::string message); + + // OnStart and OnStop are called as when the Behaviour is initially started, + // then again when the behaviour is finished. Both of these overrides are + // optional, but OnTick is mandatory. + void OnStart() override; + // OnTick is called periodically (over and over again) while the behaviour + // is running. This is where most logic goes, and also where the Behaviour + // decides whether it is done by calling SetDone(). + void OnTick(units::time::second_t dt) override; + void OnStop() override; + + private: + std::string _message; +}; + +// PrintBehaviour.cpp +#include "PrintBehaviour.h" + +using namespace behaviour; + +// Note the call to Behaviour(std::string), which is how you give a Behaviour +// a name that can be read out on Shuffleboard / in the console. You can +// also call it without a string to make an unnamed behaviour. +PrintBehaviour::PrintBehaviour(std::string message) : _message(message), Behaviour("PrintBehaviour(" + message + ")") {} + +void PrintBehaviour::OnStart() { + // When I start, I print my message + std::cout << "Hello World! My message is: " << _message << std::endl; +} + +void PrintBehaviour::OnTick() { + // I have no periodic behaviour, so I say I'm done right away. + SetDone(); +} + +void PrintBehaviour::OnStop() { + // And I say goodbye :) + std::cout << "I'm out of here, see ya!" << std::endl; +} +``` + +We can run this behaviour by doing the following: +```cpp +auto behaviour = make("Hello!"); +BehaviourScheduler::GetInstance()->Schedule(behaviour); +``` + +### Controlling systems +Let's look at how we might control a system with Behaviours. For this example, we're going to create a Behaviour that tells a flywheel shooter to spin up to a certain speed. + +First, we need to make our shooter system able to have a Behaviour. To do this, we make it implement from `HasBehaviour` - that's it, no methods to override or anything else. + +```cpp +// Shooter.h + +class Shooter : public HasBehaviour { + // ... +}; +``` + +Go ahead and register the Shooter with the BehaviourScheduler: +```cpp +void Robot::RobotInit() { + // ... + BehaviourScheduler::GetInstance()->Register(&my_shooter); + // ... +} +``` + +Next, we create our Behaviour. Just for fun, let's do some PIDF while we're at it. + +```cpp +// ShooterSpinup.h +#pragma once +#include +#include +#include + +class ShooterSpinup : public behaviour::Behaviour { + public: + ShooterSpinup(Shooter &s, units::rad_per_s speed, bool hold); + + void OnTick(units::time::second_t dt) override; + private: + Shooter &_shooter; + bool _hold; + units::rad_per_s _speed; + PIDController _pid; +}; + +// ShooterSpinup.cpp +#include "ShooterSpinup.h" + +using namespace behaviour; + +ShooterSpinup::ShooterSpinup(Shooter &s, units::rad_per_s speed, bool hold) : _shooter(s), _speed(speed), _pid(s.pid_settings), _hold(hold), Behaviour("Shooter Spinup") { + // By saying 'controls', we say that we're controlling the Shooter + // to make sure that we take over exclusive control of it. That way, + // no one else is telling it to do something different. + Controls(s); + _pid.SetSetpoint(_speed); +} + +void ShooterSpinup::OnTick(units::time::second_t dt) { + // Get the current speed from the shooter's encoder + units::rad_per_s current_speed = _shooter.gearbox.encoder.GetAngularVelocity(); + // Calculate the feedforward voltage - the voltage required to spin the + // motor assuming there is no torque (load) applied. This is simple if + // we use WPILib's DcMotor class from wpimath (#include ). + units::volt_t feed_forward = _shooter.gearbox.motor.Voltage(0, _speed); + + // Calculate the PID output from the current speed, time difference, and feed forward. + units::volt_t pid_demand = _pid.Calculate(current_speed, dt, feed_forward); + _shooter.SetVoltage(pid_demand); + + // If the PID says we're done, stop this behaviour and move on! + // Note "hold", which will keep the behaviour running even after we meet our speed + // We can use this in conjunction with ->Until(behaviour), which will keep our + // behaviour running until another behaviour is finished. + if (_pid.IsDone() && !_hold) + SetDone(); +} +``` + +See how we've taken a complex action and broken it down into a single piece of code that we can reuse? Just have a look how much easier it is to tell the shooter to spinup to an arbitrary speed: + +```cpp +auto spinup = make(my_shooter, 500_rpm, false); +``` + +We can also assign a timeout to this behaviour, in case we want to move on if it's not ready in time: +```cpp +spinup->WithTimeout(3_s); +``` + +We can also tell this behaviour to run faster by calling `SetPeriod`: +```cpp +spinup->SetPeriod(10_ms); // 100Hz +``` + +## Using Behaviours Together +As we mentioned, Behaviours are small, compartmentalised units of work that we can use together to make complex routines. In order to achieve this, Wombat provides some ways to combine behaviours together into larger sequences. + +### Sequential Execution +Behaviours can be executed in sequence by using the `<<` operator. + +```cpp +auto combined = make() + << make(); +// or +auto b1 = make(); +auto b2 = make(); +auto combined = b1 << b2; +``` + +### Parallel Execution +Behaviours can be executed together (at the same time) by using the `&` or `|` operators. Only behaviours which control different systems can be run at the same time. `&` will run until both are complete, whereas `|` will stop after either is complete (known as a "race"). + +```cpp +auto wait_both = make() & make(); +auto wait_either = make() | make(); +``` + +You can also run in parallel, waiting until a specific behaviour is complete with the `Until` function. +```cpp +auto wait_until = make() + ->Until(make()); +``` + +### Waiting +Wombat provides `WaitTime` and `WaitFor` to produce simple waits in the behaviour chain. + +```cpp +auto wait_2s = make(2_s); +// WaitFor will wait until a function (predicate) returns true before continuing. +auto wait_until_vision = make([&vision]() { return vision.ready(); }); +``` + +### Making Decisions +Making decisions in the behaviour chain is easy, as Wombat provides `If`, `Switch`, and `Decide`. + +```cpp +auto if_bhvr = make(my_bool) + ->Then(make()) + ->Else(make()); +// or, with a predicate that's evaluated when the behaviour runs +auto if_bhvr = make([&vision]() { return vision.ready(); }) + ->Then(make()) + ->Else(make()); +``` + +Switch is similar to a switch-case statement, allowing you to choose from one of many options. + +```cpp +auto switch_bhvr = make(my_int) + // Select based on the value directly + ->When(1, make()) + ->When(2, make()) + // Or, based on the value using a predicate + ->When([](auto my_int) { return my_int > 6; }, make()) + ->Otherwise(make()); + +// If no When matches, and Otherwise is not provided, the behaviour will +// keep running until one of the options matches. You can also provide +// Otherwise without an argument to exit if none match. + +// Like If, you can also provide a function to yield the initial value +auto switch_bhvr = make([]() { return my_val; }) + // ... +``` + +Decide is a special case of Switch, but without an argument - using predicates on all branches. + +```cpp +auto decide = make() + ->When([]() { return true; }, make()) + ->When([]() { return false; }, make()); +``` + +## Big picture: designing Behaviours from the Top Down. +Let's say we come up with a plan to do a really awesome (but really complicated) autonomous. The team decides the following routine is our best strategic option: +- While spinning up the shooter: + - Move forward 2m + - Intake a ball +- Shoot the ball +- Turn 90 degrees +- Wait 2 seconds +- Move forward 1.5m +- Intake a ball +- While driving backwards 3m: + - Spinup the shooter + - Wait until vision is ready + - Shoot a ball +- Intake another ball +- Spinup & Shoot the last ball + +Complex, right? Let's look at how we break it down. First of all, notice that it's already in a sequence of steps for us - small chunks of work that we can harness to complete our goals. Also notice that there's some common behaviour across this routine - there's multiple times where we move forward, intake a ball, etc. We can use this to our advantage by reducing the amount of code we need to write. + +We can use the steps we've already outlined to build our overall behaviour sequence that describes the autonomous routine. +Let's go ahead and mock up what we think our autonomous routine above will look like in code: + +```cpp +Behaviour::ptr MyAutoRoutine() { + return ( + make(shooter, 500_rpm, true) + ->Until( + make(drivetrain, 2_m) + << make(intake) + )) + << make(shooter) + << make(drivetrain, 90_deg) + << make(2_s) + << make(drivetrain, 1.5_m) + << make(intake) + << ( + // Drive Straight backwards, and + make(drivetrain, -3_m) & + ( + // Spinup the shooter until vision is ready, then fire. + make(shooter, 500_rpm, true) + ->Until(make([vision]() { return vision.ready(); })) + << make(shooter) + ) + ) + << make(intake) + << make(shooter, 500_rpm, false) + << make(shooter); +} +``` + +See how we can just flow on from the individual steps of our big, complex example? Now, instead of implementing 15 different steps in the autonomous routine, we only have to implement 5 behaviours: +- `ShooterSpinup` +- `ShooterFire` +- `DriveStraight` +- `DriveTurn` +- `IntakeOne` diff --git a/wombat/src/main/cpp/behaviour/Behaviour.cpp b/wombat/src/main/cpp/behaviour/Behaviour.cpp new file mode 100644 index 0000000..7f400a9 --- /dev/null +++ b/wombat/src/main/cpp/behaviour/Behaviour.cpp @@ -0,0 +1,261 @@ +#include "behaviour/Behaviour.h" + +using namespace behaviour; + +// Behaviour +Behaviour::Behaviour(std::string name, units::time::second_t period) + : _bhvr_name(name), _bhvr_period(period), _bhvr_state(BehaviourState::INITIALISED) {} +Behaviour::~Behaviour() { + if (!IsFinished()) Interrupt(); +} + +std::string Behaviour::GetName() const { + return _bhvr_name; +} + +void Behaviour::SetPeriod(units::time::second_t Period) { + _bhvr_period = Period; +} + +units::time::second_t Behaviour::GetPeriod() const { + return _bhvr_period; +} + +units::time::second_t Behaviour::GetRunTime() const { + return _bhvr_timer; +} + +void Behaviour::Controls(HasBehaviour *sys) { + if (sys != nullptr) _bhvr_controls.insert(sys); +} + +void Behaviour::Inherit(Behaviour &bhvr) { + for (auto c : bhvr.GetControlled()) Controls(c); +} + +Behaviour::ptr Behaviour::WithTimeout(units::time::second_t timeout) { + _bhvr_timeout = timeout; + return shared_from_this(); +} + +wpi::SmallPtrSetImpl &Behaviour::GetControlled() { + return _bhvr_controls; +} + +BehaviourState Behaviour::GetBehaviourState() const { + return _bhvr_state; +} + +void Behaviour::Interrupt() { + Stop(BehaviourState::INTERRUPTED); +} + +void Behaviour::SetDone() { + Stop(BehaviourState::DONE); +} + +bool Behaviour::Tick() { + if (_bhvr_state == BehaviourState::INITIALISED) { + _bhvr_time = frc::RobotController::GetFPGATime(); + _bhvr_state = BehaviourState::RUNNING; + _bhvr_timer = 0_s; + + OnStart(); + } + + if (_bhvr_state == BehaviourState::RUNNING) { + uint64_t now = frc::RobotController::GetFPGATime(); + auto dt = static_cast(now - _bhvr_time) / 1000000 * 1_s; + _bhvr_time = now; + _bhvr_timer += dt; + + if (dt > 2 * _bhvr_period) { + std::cerr << "Behaviour missed deadline. Reduce Period. Dt=" << dt.value() + << " Dt(deadline)=" << (2 * _bhvr_period).value() << ". Bhvr: " << GetName() << std::endl; + } + + if (_bhvr_timeout.value() > 0 && _bhvr_timer > _bhvr_timeout) + Stop(BehaviourState::TIMED_OUT); + else + OnTick(dt); + } + + return IsFinished(); +} + +bool Behaviour::IsRunning() const { + return _bhvr_state == BehaviourState::RUNNING; +} + +bool Behaviour::IsFinished() const { + return _bhvr_state != BehaviourState::INITIALISED && _bhvr_state != BehaviourState::RUNNING; +} + +void Behaviour::Stop(BehaviourState new_state) { + if (_bhvr_state.exchange(new_state) == BehaviourState::RUNNING) OnStop(); +} + +Behaviour::ptr Behaviour::Until(Behaviour::ptr other) { + // return shared_from_this() | other; + auto conc = make(ConcurrentBehaviourReducer::FIRST); + conc->Add(other); + conc->Add(shared_from_this()); + return conc; +} + +// Sequential Behaviour +void SequentialBehaviour::Add(ptr next) { + _queue.push_back(next); + Inherit(*next); +} + +std::string SequentialBehaviour::GetName() const { + return _queue.front()->GetName(); +} + +void SequentialBehaviour::OnTick(units::time::second_t dt) { + if (!_queue.empty()) { + SetPeriod(_queue.front()->GetPeriod()); + _queue.front()->Tick(); + if (_queue.front()->IsFinished()) { + _queue.pop_front(); + if (_queue.empty()) + SetDone(); + else + _queue.front()->Tick(); + } + } else { + SetDone(); + } +} + +void SequentialBehaviour::OnStop() { + if (GetBehaviourState() != BehaviourState::DONE) { + while (!_queue.empty()) { + _queue.front()->Interrupt(); + _queue.pop_front(); + } + } +} + +// ConcurrentBehaviour +ConcurrentBehaviour::ConcurrentBehaviour(ConcurrentBehaviourReducer reducer) + : Behaviour(), _reducer(reducer) {} + +void ConcurrentBehaviour::Add(Behaviour::ptr behaviour) { + for (auto c : behaviour->GetControlled()) { + auto &controls = GetControlled(); + if (controls.find(c) != controls.end()) { + throw DuplicateControlException( + "Cannot run behaviours with the same controlled system concurrently (duplicate in: " + + behaviour->GetName() + ")"); + } + Controls(c); + } + + _children.push_back(behaviour); + _children_finished.emplace_back(false); +} + +std::string ConcurrentBehaviour::GetName() const { + std::string msg = (_reducer == ConcurrentBehaviourReducer::ALL ? "ALL { " : "RACE {"); + for (auto b : _children) msg += b->GetName() + ", "; + msg += "}"; + return msg; +} + +void ConcurrentBehaviour::OnStart() { + for (size_t i = 0; i < _children.size(); i++) { + auto b = _children[i]; + + _threads.emplace_back([i, b, this]() { + while (!b->IsFinished() && !IsFinished()) { + using namespace std::chrono_literals; + + b->Tick(); + std::this_thread::sleep_for(std::chrono::milliseconds((int64_t)(b->GetPeriod().value() * 1000))); + } + + if (IsFinished() && !b->IsFinished()) b->Interrupt(); + + { + std::lock_guard lk(_children_finished_mtx); + _children_finished[i] = true; + } + }); + } +} + +void ConcurrentBehaviour::OnTick(units::time::second_t dt) { + bool ok = _reducer == ConcurrentBehaviourReducer::ALL ? true : false; + + { + std::lock_guard lk(_children_finished_mtx); + + if (_reducer == ConcurrentBehaviourReducer::FIRST) { + ok = _children_finished[0]; + } else { + for (bool fin : _children_finished) { + if (_reducer == ConcurrentBehaviourReducer::ALL) { + ok = ok && fin; + } else if (_reducer == ConcurrentBehaviourReducer::ANY) { + ok = ok || fin; + } + } + } + } + + if (ok) SetDone(); +} + +void ConcurrentBehaviour::OnStop() { + for (auto &t : _threads) { + t.join(); + } +} + +// If +If::If(std::function condition) : _condition(condition) {} +If::If(bool v) : _condition([v]() { return v; }) {} + +std::shared_ptr If::Then(Behaviour::ptr b) { + _then = b; + Inherit(*b); + return std::reinterpret_pointer_cast(shared_from_this()); +} + +std::shared_ptr If::Else(Behaviour::ptr b) { + _else = b; + Inherit(*b); + return std::reinterpret_pointer_cast(shared_from_this()); +} + +void If::OnStart() { + _value = _condition(); +} + +void If::OnTick(units::time::second_t dt) { + Behaviour::ptr _active = _value ? _then : _else; + if (_active) _active->Tick(); + if (!_active || _active->IsFinished()) SetDone(); + + if (IsFinished() && _active && !_active->IsFinished()) _active->Interrupt(); +} + +// WaitFor +WaitFor::WaitFor(std::function predicate) : _predicate(predicate) {} +void WaitFor::OnTick(units::time::second_t dt) { + if (_predicate()) SetDone(); +} + +// WaitTime +WaitTime::WaitTime(units::time::second_t time) : WaitTime([time]() { return time; }) {} +WaitTime::WaitTime(std::function time_fn) : _time_fn(time_fn) {} + +void WaitTime::OnStart() { + _time = _time_fn(); +} + +void WaitTime::OnTick(units::time::second_t dt) { + if (GetRunTime() > _time) SetDone(); +} \ No newline at end of file diff --git a/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp new file mode 100644 index 0000000..8e42972 --- /dev/null +++ b/wombat/src/main/cpp/behaviour/BehaviourScheduler.cpp @@ -0,0 +1,73 @@ +#include "behaviour/BehaviourScheduler.h" + +using namespace behaviour; + +BehaviourScheduler::BehaviourScheduler() {} + +BehaviourScheduler::~BehaviourScheduler() { + for (HasBehaviour *sys : _systems) { + if (sys->_active_behaviour) sys->_active_behaviour->Interrupt(); + } + + for (auto &t : _threads) { + t.join(); + } +} + +BehaviourScheduler *_scheduler_instance; + +BehaviourScheduler *BehaviourScheduler::GetInstance() { + if (_scheduler_instance == nullptr) + _scheduler_instance = new BehaviourScheduler(); + return _scheduler_instance; +} + +void BehaviourScheduler::Register(HasBehaviour *system) { + _systems.push_back(system); +} + +void BehaviourScheduler::Schedule(Behaviour::ptr behaviour) { + if (behaviour->GetBehaviourState() != BehaviourState::INITIALISED) { + throw std::invalid_argument("Cannot reuse Behaviours!"); + } + + std::lock_guard lk(_active_mtx); + + for (HasBehaviour *sys : behaviour->GetControlled()) { + if (sys->_active_behaviour != nullptr) sys->_active_behaviour->Interrupt(); + sys->_active_behaviour = behaviour; + + _threads.emplace_back([behaviour, this]() { + while (!behaviour->IsFinished()) { + using namespace std::chrono_literals; + + { + std::lock_guard lk(_active_mtx); + behaviour->Tick(); + } + std::this_thread::sleep_for(std::chrono::milliseconds( + (int64_t)(behaviour->GetPeriod().value() * 1000))); + } + }); + } +} + +void BehaviourScheduler::Tick() { + std::lock_guard lk(_active_mtx); + for (HasBehaviour *sys : _systems) { + if (sys->_active_behaviour->IsFinished()) { + if (sys->_default_behaviour_producer == nullptr) { + sys->_active_behaviour = nullptr; + } else { + Schedule(sys->_default_behaviour_producer()); + } + } + } +} + +void BehaviourScheduler::InterruptAll() { + std::lock_guard lk(_active_mtx); + for (HasBehaviour *sys : _systems) { + sys->_active_behaviour->Interrupt(); + } +} \ No newline at end of file diff --git a/wombat/src/main/cpp/behaviour/HasBehaviour.cpp b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp new file mode 100644 index 0000000..b279290 --- /dev/null +++ b/wombat/src/main/cpp/behaviour/HasBehaviour.cpp @@ -0,0 +1,14 @@ +#include "behaviour/HasBehaviour.h" + +#include "behaviour/Behaviour.h" + +using namespace behaviour; + +void HasBehaviour::SetDefaultBehaviour( + std::function(void)> fn) { + _default_behaviour_producer = fn; +} + +std::shared_ptr HasBehaviour::GetActiveBehaviour() { + return _active_behaviour; +} \ No newline at end of file diff --git a/wombat/src/main/include/behaviour/Behaviour.h b/wombat/src/main/include/behaviour/Behaviour.h new file mode 100644 index 0000000..6ba4e5e --- /dev/null +++ b/wombat/src/main/include/behaviour/Behaviour.h @@ -0,0 +1,455 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HasBehaviour.h" + +namespace behaviour { +enum class BehaviourState { + INITIALISED, + RUNNING, + DONE, + TIMED_OUT, + INTERRUPTED +}; + +class SequentialBehaviour; + +/** + * A Behaviour is a single component in a chain of actions. Behaviours are used + * to implement robot functionality and may include everything from running the + * intake to following trajectories and scoring game pieces. + * + * Behaviours are ideally small components that are built into a much larger + * chain, allowing each element to be tested and developed individually. + * + * For examples, see the BEHAVIOURS.md document. + */ +class Behaviour : public std::enable_shared_from_this { + public: + using ptr = std::shared_ptr; + + Behaviour(std::string name = "", + units::time::second_t period = 20_ms); + ~Behaviour(); + + /** + * @return std::string The name of the Behaviour + */ + virtual std::string GetName() const; + + /** + * Called when the Behaviour first starts + */ + virtual void OnStart(){}; + + /** + * Called periodically as the Behaviour runs + * @param dt The time difference between the current call + * and the previous call of OnTick + */ + virtual void OnTick(units::time::second_t dt) = 0; + + /** + * Called when the Behaviour stops running + */ + virtual void OnStop(){}; + + /** + * Set the period of the Behaviour. Note this only affects the Behaviour + * when scheduled using the BehaviourScheduler, or when used in as part + * of a concurrent behaviour group (& or |) + */ + void SetPeriod(units::time::second_t period); + + /** + * @return units::time::second_t The loop period of the Behaviour. + */ + units::time::second_t GetPeriod() const; + + /** + * @return units::time::second_t The amount of time the Behaviour has been + * running for. + */ + units::time::second_t GetRunTime() const; + + /** + * Specify what systems this Behaviour Controls. Controls means a physical + * output, a demand, or some other controlling method. When Behaviours run, + * only one Behaviour at a time may have control over a system. + */ + void Controls(HasBehaviour *sys); + + /** + * Inherit controlled systems from another Behaviour. + */ + void Inherit(Behaviour &bhvr); + + /** + * Set a timeout on this Behaviour. If the Behaviour runs longer than the + * timeout, it will be interrupted. + */ + ptr WithTimeout(units::time::second_t timeout); + + /** + * @return wpi::SmallPtrSetImpl& The systems controlled by + * this behaviour. + */ + wpi::SmallPtrSetImpl &GetControlled(); + + /** + * @return BehaviourState The current state of the behaviour + * @see BehaviourState + */ + BehaviourState GetBehaviourState() const; + + /** + * Interrupt this behaviour + */ + void Interrupt(); + + /** + * Set this behaviour as being complete + */ + void SetDone(); + + /** + * Tick this behaviour manually. It is very rare that you need to call this + * function, as it will usually be done automatically by the + * BehaviourScheduler. The only exception is when calling a behaviour within + * another behaviour. + */ + bool Tick(); + + /** + * Is this behaviour still running? + */ + bool IsRunning() const; + + /** + * Is this behaviour finished? + */ + bool IsFinished() const; + + /** + * Run this behaviour until another has finished. + */ + Behaviour::ptr Until(Behaviour::ptr other); + + + private: + void Stop(BehaviourState new_state); + + std::string _bhvr_name; + units::time::second_t _bhvr_period = 20_ms; + std::atomic _bhvr_state; + + wpi::SmallSet _bhvr_controls; + + double _bhvr_time = 0; + units::time::second_t _bhvr_timer = 0_s; + units::time::second_t _bhvr_timeout = -1_s; +}; + +/** + * Shorthand function to create a shared_ptr for a Behaviour. + */ +template +std::shared_ptr make(Args &&...args) { + return std::make_shared(std::forward(args)...); +} + +/** + * The SequentialBehaviour runs a number of behaviours back-to-back, to + * create a sequential chain of execution. Usually, you don't + * want to invoke this class directly, but instead use b1 << b2. + */ +class SequentialBehaviour : public Behaviour { + public: + void Add(ptr next); + + std::string GetName() const override; + + void OnTick(units::time::second_t dt) override; + void OnStop() override; + + protected: + std::deque _queue; +}; + +inline std::shared_ptr operator<<(Behaviour::ptr a, + Behaviour::ptr b) { + auto seq = std::make_shared(); + seq->Add(a); + seq->Add(b); + return seq; +} + +inline std::shared_ptr operator<<( + std::shared_ptr a, Behaviour::ptr b) { + a->Add(b); + return a; +} + +class DuplicateControlException : public std::exception { + public: + DuplicateControlException(const std::string &msg) : _msg(msg) {} + const char *what() const noexcept override { return _msg.c_str(); } + + private: + std::string _msg; +}; + +enum class ConcurrentBehaviourReducer { ALL, ANY, FIRST }; + +/** + * Create a concurrent set of behaviours that will run together. + * Usually, you don't want to call this directly, but instead use b1 & b2 or b1 + * | b2 to create a concurrent group. + */ +class ConcurrentBehaviour : public Behaviour { + public: + ConcurrentBehaviour(ConcurrentBehaviourReducer reducer); + + void Add(Behaviour::ptr behaviour); + + std::string GetName() const override; + + void OnStart() override; + void OnTick(units::time::second_t dt) override; + void OnStop() override; + + private: + ConcurrentBehaviourReducer _reducer; + std::vector> _children; + std::mutex _children_finished_mtx; + std::vector _children_finished; + std::vector _threads; +}; + +/** + * Create a concurrent behaviour group, waiting for all behaviours + * to finish before moving on. + */ +inline std::shared_ptr operator&(Behaviour::ptr a, + Behaviour::ptr b) { + auto conc = + std::make_shared(ConcurrentBehaviourReducer::ALL); + conc->Add(a); + conc->Add(b); + return conc; +} + +/** + * Create a concurrent behaviour group, where unfinished behaviours will + * be interrupted as soon as any members of the group are finished (the + * behaviours are 'raced' against each other). + */ +inline std::shared_ptr operator|(Behaviour::ptr a, + Behaviour::ptr b) { + auto conc = + std::make_shared(ConcurrentBehaviourReducer::ANY); + conc->Add(a); + conc->Add(b); + return conc; +} + +/** + * If allows decisions to be made in a behaviour chain. + */ +struct If : public Behaviour { + public: + /** + * Create a new If decision behaviour. + * @param condition The condition to check, called when the behaviour is + * scheduled. + */ + If(std::function condition); + /** + * Create a new If decision behaviour + * @param v The condition to check + */ + If(bool v); + + /** + * Set the behaviour to be called if the condition is true + */ + std::shared_ptr Then(Behaviour::ptr b); + + /** + * Set the behaviour to be called if the condition is false + */ + std::shared_ptr Else(Behaviour::ptr b); + + void OnStart() override; + void OnTick(units::time::second_t dt) override; + + private: + std::function _condition; + bool _value; + Behaviour::ptr _then, _else; +}; + +/** + * The Switch behaviour is used to select from one of multiple paths, + * depending on the value of a parameter. + * + * @tparam T The type of parameter. + */ +template +struct Switch : public Behaviour { + public: + /** + * Create a new Switch behaviour, with a given parameter + * @param fn The function yielding the parameter, called in OnTick + */ + Switch(std::function fn) : _fn(fn) {} + /** + * Create a new Switch behaviour, with a given parameter + * @param v The parameter on which decisions are made + */ + Switch(T v) : Switch([v]() { return v; }) {} + + /** + * Add a new option to the Switch chain. + * + * @param condition The function yielding true if this is the correct option + * @param b The behaviour to call if this option is provided. + */ + std::shared_ptr When(std::function condition, + Behaviour::ptr b) { + _options.push_back(std::pair(condition, b)); + Inherit(*b); + return std::reinterpret_pointer_cast>(shared_from_this()); + } + + /** + * Add a new option to the Switch chain. + * + * @param value The option value, to be checked against the parameter + * @param b The behaviour to call if this option is provided. + */ + std::shared_ptr When(T value, Behaviour::ptr b) { + return When([value](T &v) { return value == v; }, b); + } + + /** + * Set the default branch for the Switch chain + * @param b The behaviour to call if no other When's match. + */ + std::shared_ptr Otherwise(Behaviour::ptr b = nullptr) { + return When([](T &v) { return true; }, b); + } + + void OnTick(units::time::second_t dt) override { + T val = _fn(); + + if (!_locked) { + for (auto &opt : _options) { + if (opt.first(val)) { + _locked = opt.second; + + if (_locked == nullptr) + SetDone(); + } + } + } + + if (_locked) { + SetPeriod(_locked->GetPeriod()); + if (_locked->Tick()) { + SetDone(); + } + } + } + + void OnStop() override { + if (GetBehaviourState() != BehaviourState::DONE) { + for (auto &opt : _options) { + opt.second->Interrupt(); + } + } + } + + private: + std::function _fn; + wpi::SmallVector, Behaviour::ptr>, 4> + _options; + Behaviour::ptr _locked = nullptr; +}; + +/** + * The Decide behaviour is a special case of Switch, where no parameter is + * provided and is instead based purely on predicates. + */ +struct Decide : public Switch { + Decide() : Switch(std::monostate{}){}; + + /** + * Add a new option to the Switch chain. + * + * @param condition The function yielding true if this is the correct option + * @param b The behaviour to call if this option is provided. + */ + std::shared_ptr When(std::function condition, + Behaviour::ptr b) { + return std::reinterpret_pointer_cast( + Switch::When([condition](auto) { return condition(); }, b)); + } +}; + +/** + * The WaitFor behaviour will do nothing until a condition is true. + */ +struct WaitFor : public Behaviour { + public: + /** + * Create a new WaitFor behaviour + * @param predicate The condition predicate + */ + WaitFor(std::function predicate); + + void OnTick(units::time::second_t dt) override; + + private: + std::function _predicate; +}; + +/** + * The WaitTime behaviour will do nothing until a time period has elapsed. + */ +struct WaitTime : public Behaviour { + public: + /** + * Create a new WaitTime behaviour + * @param time The time period to wait + */ + WaitTime(units::time::second_t time); + + /** + * Create a new WaitTime behaviour + * @param time_fn The time period to wait, evaluated at OnStart + */ + WaitTime(std::function time_fn); + + void OnStart() override; + void OnTick(units::time::second_t dt) override; + + private: + std::function _time_fn; + units::time::second_t _time; +}; +} // namespace behaviour diff --git a/wombat/src/main/include/behaviour/BehaviourScheduler.h b/wombat/src/main/include/behaviour/BehaviourScheduler.h new file mode 100644 index 0000000..8ee2399 --- /dev/null +++ b/wombat/src/main/include/behaviour/BehaviourScheduler.h @@ -0,0 +1,56 @@ +#pragma once + +#include + +#include "Behaviour.h" +#include "HasBehaviour.h" + +namespace behaviour { + +/** + * The BehaviourScheduler is the primary entrypoint for running behaviours. + * Behaviours are scheduled with Schedule(...), and systems are registered with + * Register(...). + * + * The scheduler Tick() method must be called on a regular basis, such as in + * RobotPeriodic + */ +class BehaviourScheduler { + public: + BehaviourScheduler(); + ~BehaviourScheduler(); + + /** + * @return BehaviourScheduler* The global instance of the BehaviourScheduler + */ + static BehaviourScheduler *GetInstance(); + + /** + * Register a system with the behaviour scheduler. A system must be registered + * for it to be controlled by behaviours. + */ + void Register(HasBehaviour *system); + + /** + * Schedule a behaviour, interrupting all behaviours currently running that + * control the same system. + */ + void Schedule(Behaviour::ptr behaviour); + + /** + * Update the BehaviourScheduler. Must be called regularly, e.g. RobotPeriodic + */ + void Tick(); + + /** + * Interrupt all running behaviours. This is commonly called on DisabledInit + * and TeleopInit. + */ + void InterruptAll(); + + private: + std::vector _systems; + std::recursive_mutex _active_mtx; + std::vector _threads; +}; +} // namespace behaviour \ No newline at end of file diff --git a/wombat/src/main/include/behaviour/HasBehaviour.h b/wombat/src/main/include/behaviour/HasBehaviour.h new file mode 100644 index 0000000..cfe0b38 --- /dev/null +++ b/wombat/src/main/include/behaviour/HasBehaviour.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +namespace behaviour { +class Behaviour; +class BehaviourScheduler; + +/** + * HasBehaviour is applied to a system that can be controlled by behaviours. + * This is commonly implemented on shooters, drivetrains, elevators, etc. + */ +class HasBehaviour { + public: + /** + * Set the default behaviour to run if no behaviours are currently running. + * This is commonly used to default to Teleoperated control. + */ + void SetDefaultBehaviour(std::function(void)> fn); + + /** + * Get the currently running behaviour on this system. + */ + std::shared_ptr GetActiveBehaviour(); + + protected: + std::shared_ptr _active_behaviour; + std::function(void)> _default_behaviour_producer; + + private: + friend class BehaviourScheduler; +}; +} // namespace behaviour \ No newline at end of file diff --git a/wombat/src/test/cpp/test_Behaviour.cpp b/wombat/src/test/cpp/test_Behaviour.cpp new file mode 100644 index 0000000..70bdbe6 --- /dev/null +++ b/wombat/src/test/cpp/test_Behaviour.cpp @@ -0,0 +1,366 @@ +#include +#include + +#include "behaviour/Behaviour.h" +#include "behaviour/BehaviourScheduler.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using namespace behaviour; + +class MockSystem : public HasBehaviour {}; +class MockBehaviour : public Behaviour { + public: + MOCK_METHOD0(OnStart, void()); + MOCK_METHOD0_T(OnStop, void()); + MOCK_METHOD1(OnTick, void(units::time::second_t)); +}; + +TEST(Behaviour, Tick) { + auto b = make(); + + { + ::testing::InSequence s; + EXPECT_CALL(*b, OnStart).Times(1); + EXPECT_CALL(*b, OnTick).Times(4); + EXPECT_CALL(*b, OnStop).Times(1); + } + + EXPECT_FALSE(b->Tick()); + EXPECT_FALSE(b->Tick()); + EXPECT_FALSE(b->Tick()); + EXPECT_FALSE(b->Tick()); + b->SetDone(); + EXPECT_TRUE(b->Tick()); + EXPECT_EQ(b->GetBehaviourState(), BehaviourState::DONE); +} + +TEST(Behaviour, Interrupt) { + auto b = make(); + + { + ::testing::InSequence s; + EXPECT_CALL(*b, OnStart).Times(1); + EXPECT_CALL(*b, OnTick).Times(2); + EXPECT_CALL(*b, OnStop).Times(1); + } + + EXPECT_FALSE(b->Tick()); + EXPECT_FALSE(b->Tick()); + b->Interrupt(); + EXPECT_TRUE(b->Tick()); + EXPECT_EQ(b->GetBehaviourState(), BehaviourState::INTERRUPTED); +} + +TEST(Behaviour, Timeout) { + auto b = make(); + b->WithTimeout(10_ms); + + { + ::testing::InSequence s; + EXPECT_CALL(*b, OnStart).Times(1); + EXPECT_CALL(*b, OnTick).Times(2); + EXPECT_CALL(*b, OnStop).Times(1); + } + + EXPECT_FALSE(b->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(6)); + EXPECT_FALSE(b->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(6)); + EXPECT_TRUE(b->Tick()); +} + +TEST(SequentialBehaviour, InheritsControls) { + HasBehaviour a, b; + auto b1 = make(), b2 = make(); + b1->Controls(&a); + b2->Controls(&a); + b2->Controls(&b); + + auto chain = b1 << b2; + ASSERT_EQ(chain->GetControlled().count(&a), 1); + ASSERT_EQ(chain->GetControlled().count(&b), 1); +} + +TEST(SequentialBehaviour, Sequence) { + auto b1 = make(), b2 = make(), b3 = make(), + b4 = make(); + auto chain = b1 << b2 << b3 << b4; + + { + ::testing::InSequence s; + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(2); + EXPECT_CALL(*b1, OnStop).Times(1); + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b2, OnTick).Times(1); + EXPECT_CALL(*b2, OnStop).Times(1); + EXPECT_CALL(*b3, OnStart).Times(1); + EXPECT_CALL(*b3, OnTick).Times(1); + EXPECT_CALL(*b3, OnStop).Times(1); + } + + EXPECT_FALSE(chain->Tick()); + EXPECT_FALSE(chain->Tick()); + b1->SetDone(); + EXPECT_FALSE(chain->Tick()); + b2->Interrupt(); + EXPECT_FALSE(chain->Tick()); + chain->Interrupt(); + EXPECT_TRUE(chain->Tick()); + ASSERT_EQ(b1->GetBehaviourState(), BehaviourState::DONE); + ASSERT_EQ(b2->GetBehaviourState(), BehaviourState::INTERRUPTED); + ASSERT_EQ(b3->GetBehaviourState(), BehaviourState::INTERRUPTED); + ASSERT_EQ(b4->GetBehaviourState(), BehaviourState::INTERRUPTED); +} + +TEST(ConcurrentBehaviour, InheritsControls) { + HasBehaviour a, b; + auto b1 = make(), b2 = make(), b3 = make(); + b1->Controls(&a); + b2->Controls(&b); + b3->Controls(&a); + + auto chain1 = b1 & b2; + ASSERT_EQ(chain1->GetControlled().count(&a), 1); + ASSERT_EQ(chain1->GetControlled().count(&b), 1); + + EXPECT_THROW(b1 | b3, DuplicateControlException); +} + +TEST(ConcurrentBehaviour, Race) { + auto b1 = make(), b2 = make(); + b1->SetPeriod(20_ms); + b2->SetPeriod(10_ms); + + auto chain = b1 | b2; + chain->SetPeriod(1_s); // Silence Period warning + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(::testing::Between(4, 6)); + EXPECT_CALL(*b2, OnTick).Times(::testing::Between(9, 11)); + EXPECT_CALL(*b1, OnStop).Times(1); + EXPECT_CALL(*b2, OnStop).Times(1); + + ASSERT_FALSE(chain->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + b1->SetDone(); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + ASSERT_TRUE(chain->Tick()); + EXPECT_EQ(b1->GetBehaviourState(), BehaviourState::DONE); + EXPECT_EQ(b2->GetBehaviourState(), BehaviourState::INTERRUPTED); +} + +TEST(ConcurrentBehaviour, All) { + auto b1 = make(), b2 = make(); + b1->SetPeriod(20_ms); + b2->SetPeriod(10_ms); + + auto chain = b1 & b2; + chain->SetPeriod(1_s); // Silence Period warning + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(::testing::Between(4, 6)); + EXPECT_CALL(*b2, OnTick).Times(::testing::Between(14, 16)); + EXPECT_CALL(*b1, OnStop).Times(1); + EXPECT_CALL(*b2, OnStop).Times(1); + + ASSERT_FALSE(chain->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + b1->SetDone(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + b2->SetDone(); + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + ASSERT_TRUE(chain->Tick()); + EXPECT_EQ(b1->GetBehaviourState(), BehaviourState::DONE); + EXPECT_EQ(b2->GetBehaviourState(), BehaviourState::DONE); +} + +TEST(ConcurrentBehaviour, Until) { + auto b1 = make(), b2 = make(); + auto chain = b1->Until(b2); + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(::testing::AtLeast(1)); + EXPECT_CALL(*b1, OnStop).Times(1); + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b2, OnTick).Times(::testing::AtLeast(1)); + EXPECT_CALL(*b2, OnStop).Times(1); + + ASSERT_FALSE(chain->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + ASSERT_TRUE(b1->IsRunning()); + ASSERT_TRUE(b2->IsRunning()); + + // TODO: Need to add a way to have SetDone interrupt the wait on + // the thread. E.g. SleepOrUntilDone + b2->SetDone(); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + ASSERT_TRUE(chain->Tick()); + ASSERT_FALSE(b1->IsRunning()); + ASSERT_FALSE(b2->IsRunning()); +} + +TEST(WaitFor, Waits) { + bool v = false; + auto b = make([&v]() { return v; }); + + ASSERT_FALSE(b->Tick()); + ASSERT_FALSE(b->Tick()); + v = true; + ASSERT_TRUE(b->Tick()); + ASSERT_EQ(b->GetBehaviourState(), BehaviourState::DONE); +} + +TEST(WaitTime, Waits) { + auto b = make(20_ms); + + ASSERT_FALSE(b->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(11)); + ASSERT_FALSE(b->Tick()); + std::this_thread::sleep_for(std::chrono::milliseconds(11)); + ASSERT_TRUE(b->Tick()); + ASSERT_EQ(b->GetBehaviourState(), BehaviourState::DONE); +} + +TEST(If, Then) { + auto b1 = make(), b2 = make(); + + auto chain = make(true)->Then(b1)->Else(b2); + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(2); + + chain->Tick(); + chain->Tick(); +} + +TEST(If, Else) { + auto b1 = make(), b2 = make(); + + auto chain = make(false)->Then(b1)->Else(b2); + + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b2, OnTick).Times(2); + + chain->Tick(); + chain->Tick(); +} + +TEST(Switch, Int) { + auto b1 = make(), b2 = make(), b3 = make(); + + auto chain = make>(1)->When(0, b1)->When(1, b2)->When(2, b3); + + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b2, OnTick).Times(2); + + chain->Tick(); + chain->Tick(); +} + +TEST(Switch, Decide) { + auto b1 = make(), b2 = make(), b3 = make(); + + auto chain = make() + ->When([]() { return true; }, b1) + ->When([]() { return false; }, b2) + ->When([]() { return false; }, b3); + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b1, OnTick).Times(2); + + chain->Tick(); + chain->Tick(); +} + +TEST(Behaviour, FullChain) { + BehaviourScheduler s; + MockSystem a, b; + auto b1 = make(), b2 = make(), b3 = make(), + b4 = make(); + + b1->Controls(&a); + b2->Controls(&b); + b3->Controls(&a); + + b1->SetPeriod(10_ms); + b2->SetPeriod(50_ms); + b3->SetPeriod(25_ms); + b4->SetPeriod(1_s / 75.0); + + EXPECT_CALL(*b1, OnStart).Times(1); + EXPECT_CALL(*b2, OnStart).Times(1); + EXPECT_CALL(*b3, OnStart).Times(1); + EXPECT_CALL(*b4, OnStart).Times(1); + + EXPECT_CALL(*b1, OnStop).Times(1); + EXPECT_CALL(*b2, OnStop).Times(1); + EXPECT_CALL(*b3, OnStop).Times(1); + EXPECT_CALL(*b4, OnStop).Times(1); + + EXPECT_CALL(*b1, OnTick).Times(::testing::Between(9, 11)); // 100ms @ 100Hz + EXPECT_CALL(*b2, OnTick).Times(::testing::Between(5, 7)); // 300ms @ 20Hz + EXPECT_CALL(*b3, OnTick).Times(::testing::Between(4, 6)); // 100ms @ 50Hz + EXPECT_CALL(*b4, OnTick).Times(::testing::Between(22, 23)); // 300ms @ 75Hz + + auto chain = ((b1 << b3) & b2) | b4; + + s.Tick(); + s.Register(&a); + s.Register(&b); + + ASSERT_EQ(a.GetActiveBehaviour(), nullptr); + ASSERT_EQ(b.GetActiveBehaviour(), nullptr); + + s.Schedule(chain); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + s.Tick(); + + ASSERT_EQ(a.GetActiveBehaviour(), chain); + ASSERT_EQ(b.GetActiveBehaviour(), chain); + + ASSERT_TRUE(b1->IsRunning()); + ASSERT_TRUE(b2->IsRunning()); + ASSERT_FALSE(b3->IsRunning()); + ASSERT_TRUE(b4->IsRunning()); + + b1->SetDone(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + s.Tick(); + + ASSERT_FALSE(b1->IsRunning()); + ASSERT_TRUE(b2->IsRunning()); + ASSERT_TRUE(b3->IsRunning()); + ASSERT_TRUE(b4->IsRunning()); + + b3->SetDone(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + s.Tick(); + + ASSERT_FALSE(b1->IsRunning()); + ASSERT_TRUE(b2->IsRunning()); + ASSERT_FALSE(b3->IsRunning()); + ASSERT_TRUE(b4->IsRunning()); + + b4->SetDone(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + s.Tick(); + + ASSERT_FALSE(b1->IsRunning()); + ASSERT_FALSE(b2->IsRunning()); + ASSERT_FALSE(b3->IsRunning()); + ASSERT_FALSE(b4->IsRunning()); + + ASSERT_EQ(b2->GetBehaviourState(), BehaviourState::INTERRUPTED); + ASSERT_EQ(b4->GetBehaviourState(), BehaviourState::DONE); + + ASSERT_EQ(a.GetActiveBehaviour(), nullptr); + ASSERT_EQ(b.GetActiveBehaviour(), nullptr); +}