Custom executor support

alica_engine/include/engine/AlicaContext.h

@@ -352,6 +352,18 @@ class AlicaContext
     template <class TimerFactoryType, class... Args>
     void setTimerFactory(Args&&... args);
 
+    /**
+     * Set the timer factory to be used by the main engine loop.
+     * Example usage: setEngineTimerFactory<alicaRosTimer::AlicaRosTimerFactory>();
+     *
+     * @note TimerFactoryType must be a derived class of IAlicaTimerFactory
+     * @note This must be called before initializing context
+     *
+     * @param args Arguments to be forwarded to constructor of timer factory. Might be empty.
+     */
+    template <class TimerFactoryType, class... Args>
+    void setEngineTimerFactory(Args&&... args);
+
     /**
      * Get timer factory being used by this alica instance.
      *
@@ -363,6 +375,17 @@ class AlicaContext
         return *_timerFactory;
     }
 
+    /**
+     * Get timer factory being used by the main engine loop.
+     *
+     * @return A reference to timer factory object being used by context
+     */
+    IAlicaTimerFactory& getEngineTimerFactory() const
+    {
+        assert(_engineTimerFactory.get());
+        return *_engineTimerFactory;
+    }
+
     /**
      * Set trace factory to be used by this alica framework instance.
      * Example usage: setTraceFactory<amr_tracing::TraceFactory>();
@@ -474,6 +497,7 @@ class AlicaContext
     std::unordered_map<size_t, std::unique_ptr<ISolverBase>> _solvers;
     std::unique_ptr<IAlicaTimerFactory> _timerFactory;
     std::unique_ptr<IAlicaTraceFactory> _traceFactory;
+    std::unique_ptr<IAlicaTimerFactory> _engineTimerFactory;
     static const std::unordered_map<std::string, Verbosity> _verbosityStringToVerbosityMap;
 
     bool _initialized = false;
@@ -572,6 +596,18 @@ void AlicaContext::setTimerFactory(Args&&... args)
     _timerFactory = std::make_unique<TimerFactoryType>(std::forward<Args>(args)...);
 }
 
+template <class TimerFactoryType, class... Args>
+void AlicaContext::setEngineTimerFactory(Args&&... args)
+{
+    if (_initialized) {
+        Logging::logWarn(LOGNAME) << "Context already initialized. Can not set new engine timer factory";
+        return;
+    }
+
+    static_assert(std::is_base_of<IAlicaTimerFactory, TimerFactoryType>::value, "Must be derived from IAlicaTimerFactory");
+    _engineTimerFactory = std::make_unique<TimerFactoryType>(std::forward<Args>(args)...);
+}
+
 template <class TraceFactoryType, class... Args>
 void AlicaContext::setTraceFactory(Args&&... args)
 {

alica_engine/include/engine/AlicaEngine.h

@@ -101,6 +101,7 @@ class AlicaEngine
     SolverType& getSolver() const;
     template <class SolverType>
     bool existSolver() const;
+    IAlicaTimerFactory& getEngineTimerFactory() const;
 
     void reload(const YAML::Node& config);
     //[[deprecated("temporary method tobe removed in last PR")]]

alica_engine/include/engine/PlanBase.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "engine/AlicaClock.h"
+#include "engine/IAlicaTimer.h"
 #include "engine/RuleBook.h"
 #include "engine/RunningPlan.h"
 #include "engine/RuntimeBehaviourFactory.h"
@@ -12,6 +13,7 @@
 #include <math.h>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <queue>
 #include <stdio.h>
 #include <thread>
@@ -48,7 +50,7 @@ class PlanBase
             SyncModule& syncModule, AuthorityManager& authorityManager, TeamObserver& teamObserver, TeamManager& teamManager,
             const PlanRepository& planRepository, std::atomic<bool>& stepEngine, std::atomic<bool>& stepCalled, std::shared_ptr<Blackboard> globalBlackboard,
             VariableSyncModule& resultStore, const std::unordered_map<size_t, std::unique_ptr<ISolverBase>>& solvers, const IAlicaTimerFactory& timerFactory,
-            const IAlicaTraceFactory* traceFactory);
+            const IAlicaTraceFactory* traceFactory, const IAlicaTimerFactory& engineTimerFactory);
     ~PlanBase();
     RunningPlan* getRootNode() const { return _runningPlans.empty() ? nullptr : _runningPlans[0].get(); }
     PlanSelector* getPlanSelector() const { return _ruleBook.getPlanSelector(); }
@@ -58,6 +60,7 @@ class PlanBase
     void setLoopInterval(AlicaTime loopInterval);
     void stop();
     void start(const Plan* masterPlan);
+    void run(const Plan* masterPlan);
     void addFastPathEvent(RunningPlan* p);
     bool isWaiting() const { return _isWaiting; }
 
@@ -75,8 +78,6 @@ class PlanBase
 private:
     static constexpr const char* LOGNAME = "PlanBase";
 
-    void run(const Plan* masterPlan);
-
     // Owning container of running plans (replace with uniqueptrs once possible)
     std::vector<std::shared_ptr<RunningPlan>> _runningPlans;
 
@@ -101,10 +102,11 @@ class PlanBase
     VariableSyncModule& _resultStore;
     const std::unordered_map<size_t, std::unique_ptr<ISolverBase>>& _solvers;
     RunningPlan* _rootNode;
+    const IAlicaTimerFactory& _engineTimerFactory;
 
     const RunningPlan* _deepestNode;
 
-    std::unique_ptr<std::thread> _mainThread;
+    std::unique_ptr<IAlicaTimer> _engineTimer;
     std::unique_ptr<AlicaEngineInfo> _statusMessage;
 
     AlicaTime _loopTime;

alica_engine/src/engine/AlicaContext.cpp

@@ -1,5 +1,6 @@
 #include "engine/AlicaContext.h"
 #include "engine/AlicaEngine.h"
+#include "engine/AlicaTimer.h"
 #include "engine/Types.h"
 #include "engine/constraintmodul/VariableSyncModule.h"
 #include "engine/logging/AlicaDefaultLogger.h"
@@ -89,15 +90,21 @@ int AlicaContext::init(AlicaCreators&& creatorCtx, bool delayStart)
     if (!_timerFactory) {
         AlicaEngine::abort(LOGNAME, "TimerFactory not set");
     }
+    if (!_engineTimerFactory) {
+        Logging::logWarn(LOGNAME) << "Engine TimerFactory not set, defaulting to AlicaSystemTimerFactory";
+        _engineTimerFactory = std::make_unique<alica::AlicaSystemTimerFactory>();
+    }
 
     _engine = std::make_unique<AlicaEngine>(*this, _configRootNode, _alicaContextParams);
 
     _communicator->startCommunication();
 
     if (_engine->init(std::move(creatorCtx))) {
-        LockedBlackboardRW gbb(*_globalBlackboard);
-        gbb.set("agentName", _engine->getLocalAgentName());
-        gbb.set("agentId", _engine->getTeamManager().getLocalAgentID());
+        {
+            LockedBlackboardRW gbb(*_globalBlackboard);
+            gbb.set("agentName", _engine->getLocalAgentName());
+            gbb.set("agentId", _engine->getTeamManager().getLocalAgentID());
+        }
         if (!delayStart) {
             _engine->start();
         } else {

alica_engine/src/engine/AlicaEngine.cpp

@@ -45,7 +45,7 @@ AlicaEngine::AlicaEngine(AlicaContext& ctx, YAML::Node& config, const AlicaConte
                   _configChangeListener, _ctx.getCommunicator(), _ctx.getAlicaClock(), editTeamManager(), _ctx.getTimerFactory()))
         , _planBase(_configChangeListener, _ctx.getAlicaClock(), _ctx.getCommunicator(), editRoleAssignment(), editSyncModul(), editAuth(), editTeamObserver(),
                   editTeamManager(), getPlanRepository(), _stepEngine, _stepCalled, getGlobalBlackboard(), editResultStore(), _ctx.getSolvers(),
-                  getTimerFactory(), getTraceFactory())
+                  getTimerFactory(), getTraceFactory(), getEngineTimerFactory())
 {
     auto reloadFunctionPtr = std::bind(&AlicaEngine::reload, this, std::placeholders::_1);
     _configChangeListener.subscribe(reloadFunctionPtr);
@@ -165,6 +165,11 @@ IAlicaTimerFactory& AlicaEngine::getTimerFactory() const
     return _ctx.getTimerFactory();
 }
 
+IAlicaTimerFactory& AlicaEngine::getEngineTimerFactory() const
+{
+    return _ctx.getEngineTimerFactory();
+}
+
 /**
  * Register with this EngineTrigger to be called after an engine iteration is complete.
  */