diff --git a/alica_engine/include/engine/AlicaContext.h b/alica_engine/include/engine/AlicaContext.h index c218cad45..cf8140ea7 100644 --- a/alica_engine/include/engine/AlicaContext.h +++ b/alica_engine/include/engine/AlicaContext.h @@ -352,6 +352,18 @@ class AlicaContext template void setTimerFactory(Args&&... args); + /** + * Set the timer factory to be used by the main engine loop. + * Example usage: setEngineTimerFactory(); + * + * @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 + 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(); @@ -474,6 +497,7 @@ class AlicaContext std::unordered_map> _solvers; std::unique_ptr _timerFactory; std::unique_ptr _traceFactory; + std::unique_ptr _engineTimerFactory; static const std::unordered_map _verbosityStringToVerbosityMap; bool _initialized = false; @@ -572,6 +596,18 @@ void AlicaContext::setTimerFactory(Args&&... args) _timerFactory = std::make_unique(std::forward(args)...); } +template +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::value, "Must be derived from IAlicaTimerFactory"); + _engineTimerFactory = std::make_unique(std::forward(args)...); +} + template void AlicaContext::setTraceFactory(Args&&... args) { diff --git a/alica_engine/include/engine/AlicaEngine.h b/alica_engine/include/engine/AlicaEngine.h index c38a0687f..1feb90ec2 100644 --- a/alica_engine/include/engine/AlicaEngine.h +++ b/alica_engine/include/engine/AlicaEngine.h @@ -101,6 +101,7 @@ class AlicaEngine SolverType& getSolver() const; template bool existSolver() const; + IAlicaTimerFactory& getEngineTimerFactory() const; void reload(const YAML::Node& config); //[[deprecated("temporary method tobe removed in last PR")]] diff --git a/alica_engine/include/engine/PlanBase.h b/alica_engine/include/engine/PlanBase.h index f90ea9955..3866fd239 100644 --- a/alica_engine/include/engine/PlanBase.h +++ b/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 #include #include +#include #include #include #include @@ -48,7 +50,7 @@ class PlanBase SyncModule& syncModule, AuthorityManager& authorityManager, TeamObserver& teamObserver, TeamManager& teamManager, const PlanRepository& planRepository, std::atomic& stepEngine, std::atomic& stepCalled, std::shared_ptr globalBlackboard, VariableSyncModule& resultStore, const std::unordered_map>& 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> _runningPlans; @@ -101,10 +102,11 @@ class PlanBase VariableSyncModule& _resultStore; const std::unordered_map>& _solvers; RunningPlan* _rootNode; + const IAlicaTimerFactory& _engineTimerFactory; const RunningPlan* _deepestNode; - std::unique_ptr _mainThread; + std::unique_ptr _engineTimer; std::unique_ptr _statusMessage; AlicaTime _loopTime; diff --git a/alica_engine/src/engine/AlicaContext.cpp b/alica_engine/src/engine/AlicaContext.cpp index 3f0ab68f0..bc2f75163 100644 --- a/alica_engine/src/engine/AlicaContext.cpp +++ b/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(); + } _engine = std::make_unique(*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 { diff --git a/alica_engine/src/engine/AlicaEngine.cpp b/alica_engine/src/engine/AlicaEngine.cpp index 1fe617a25..ed0aeb417 100644 --- a/alica_engine/src/engine/AlicaEngine.cpp +++ b/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. */ diff --git a/alica_engine/src/engine/PlanBase.cpp b/alica_engine/src/engine/PlanBase.cpp index 9ee2bf816..365dc10f7 100644 --- a/alica_engine/src/engine/PlanBase.cpp +++ b/alica_engine/src/engine/PlanBase.cpp @@ -33,7 +33,7 @@ PlanBase::PlanBase(ConfigChangeListener& configChangeListener, const AlicaClock& IRoleAssignment& roleAssignment, SyncModule& syncModule, AuthorityManager& authorityManager, TeamObserver& teamObserver, TeamManager& teamManager, const PlanRepository& planRepository, std::atomic& stepEngine, std::atomic& stepCalled, std::shared_ptr globalBlackboard, VariableSyncModule& resultStore, const std::unordered_map>& solvers, const IAlicaTimerFactory& timerFactory, - const IAlicaTraceFactory* traceFactory) + const IAlicaTraceFactory* traceFactory, const IAlicaTimerFactory& engineTimerFactory) : _configChangeListener(configChangeListener) , _clock(clock) , _communicator(communicator) @@ -51,8 +51,8 @@ PlanBase::PlanBase(ConfigChangeListener& configChangeListener, const AlicaClock& , _resultStore(resultStore) , _solvers(solvers) , _rootNode(nullptr) + , _engineTimerFactory(engineTimerFactory) , _deepestNode(nullptr) - , _mainThread(nullptr) , _statusMessage(nullptr) , _stepModeCV() , _ruleBook(configChangeListener, syncModule, teamObserver, teamManager, planRepository, this) @@ -124,7 +124,7 @@ void PlanBase::start(const Plan* masterPlan) _statusMessage->senderID = _teamManager.getLocalAgentID(); _statusMessage->masterPlan = masterPlan->getName(); } - _mainThread = std::make_unique(&PlanBase::run, this, masterPlan); + _engineTimer = _engineTimerFactory.createTimer([this, masterPlan]() { this->run(masterPlan); }, _loopTime); } } @@ -133,188 +133,184 @@ void PlanBase::start(const Plan* masterPlan) */ void PlanBase::run(const Plan* masterPlan) { - Logging::logDebug(LOGNAME) << "Run-Method of PlanBase started."; + AlicaTime beginTime = _clock.now(); - while (_running) { - AlicaTime beginTime = _clock.now(); - - if (_stepEngine) { + if (_stepEngine) { #ifdef ALICA_DEBUG_ENABLED - Logging::logDebug(LOGNAME) << "===CUR TREE==="; - if (_rootNode == nullptr) { - Logging::logDebug(LOGNAME) << "NULL"; - } else { - _rootNode->printRecursive(); - } - Logging::logDebug(LOGNAME) << "===END CUR TREE==="; + Logging::logDebug(LOGNAME) << "===CUR TREE==="; + if (_rootNode == nullptr) { + Logging::logDebug(LOGNAME) << "NULL"; + } else { + _rootNode->printRecursive(); + } + Logging::logDebug(LOGNAME) << "===END CUR TREE==="; #endif - { - std::unique_lock lckStep(_stepMutex); - _isWaiting = true; - _stepModeCV.wait(lckStep, [&] { return _stepCalled.load(); }); - _stepCalled = false; - _isWaiting = false; - if (!_running) { - return; - } + { + std::unique_lock lckStep(_stepMutex); + _isWaiting = true; + _stepModeCV.wait(lckStep, [&] { return _stepCalled.load(); }); + _stepCalled = false; + _isWaiting = false; + if (!_running) { + return; } - beginTime = _clock.now(); } + beginTime = _clock.now(); + } - // Send tick to other modules - //_ae->getCommunicator().tick(); // not implemented as ros works asynchronous - _teamObserver.tick(_rootNode); - _roleAssignment.tick(); - _syncModule.tick(); - _authorityManager.tick(_rootNode); - _teamManager.tick(); + // Send tick to other modules + //_ae->getCommunicator().tick(); // not implemented as ros works asynchronous + _teamObserver.tick(_rootNode); + _roleAssignment.tick(); + _syncModule.tick(); + _authorityManager.tick(_rootNode); + _teamManager.tick(); - if (_rootNode == nullptr) { - _rootNode = _ruleBook.initialisationRule(masterPlan); - } - _rootNode->preTick(); - if (_rootNode->tick(&_ruleBook) == PlanChange::FailChange) { - Logging::logInfo(LOGNAME) << "MasterPlan Failed"; - } - // clear deepest node pointer before deleting plans: - if (_deepestNode && _deepestNode->isRetired()) { - _deepestNode = nullptr; - } - // remove deletable plans: - // this should be done just before clearing fpEvents, to make sure no spurious pointers remain + if (_rootNode == nullptr) { + _rootNode = _ruleBook.initialisationRule(masterPlan); + } + _rootNode->preTick(); + if (_rootNode->tick(&_ruleBook) == PlanChange::FailChange) { + Logging::logInfo(LOGNAME) << "MasterPlan Failed"; + } + // clear deepest node pointer before deleting plans: + if (_deepestNode && _deepestNode->isRetired()) { + _deepestNode = nullptr; + } + // remove deletable plans: + // this should be done just before clearing fpEvents, to make sure no spurious pointers remain #ifdef ALICA_DEBUG_ENABLED - int retiredCount = 0; - int inActiveCount = 0; - int deleteCount = 0; - int totalCount = static_cast(_runningPlans.size()); + int retiredCount = 0; + int inActiveCount = 0; + int deleteCount = 0; + int totalCount = static_cast(_runningPlans.size()); #endif - for (int i = static_cast(_runningPlans.size()) - 1; i >= 0; --i) { + for (int i = static_cast(_runningPlans.size()) - 1; i >= 0; --i) { #ifdef ALICA_DEBUG_ENABLED - if (_runningPlans[i]->isRetired()) { - ++retiredCount; - } else if (!_runningPlans[i]->isActive()) { - ++inActiveCount; - } + if (_runningPlans[i]->isRetired()) { + ++retiredCount; + } else if (!_runningPlans[i]->isActive()) { + ++inActiveCount; + } #endif - if (_runningPlans[i]->isDeleteable()) { - assert(_runningPlans[i].use_count() == 1); - _runningPlans.erase(_runningPlans.begin() + i); + if (_runningPlans[i]->isDeleteable()) { + assert(_runningPlans[i].use_count() == 1); + _runningPlans.erase(_runningPlans.begin() + i); #ifdef ALICA_DEBUG_ENABLED - ++deleteCount; + ++deleteCount; #endif - } } + } #ifdef ALICA_DEBUG_ENABLED - Logging::logDebug(LOGNAME) << (totalCount - inActiveCount - retiredCount) << " active " << retiredCount << " retired " << inActiveCount - << " inactive deleted: " << deleteCount; + Logging::logDebug(LOGNAME) << (totalCount - inActiveCount - retiredCount) << " active " << retiredCount << " retired " << inActiveCount + << " inactive deleted: " << deleteCount; #endif - // lock for fpEvents - { - std::lock_guard lock(_lomutex); - _fpEvents = std::queue(); - } + // lock for fpEvents + { + std::lock_guard lock(_lomutex); + _fpEvents = std::queue(); + } - AlicaTime now = _clock.now(); + AlicaTime now = _clock.now(); - if (now < _lastSendTime) { - Logging::logWarn(LOGNAME) << "lastSendTime is in the future of the current system time, did the system time change?"; - _lastSendTime = now; - } + if (now < _lastSendTime) { + Logging::logWarn(LOGNAME) << "lastSendTime is in the future of the current system time, did the system time change?"; + _lastSendTime = now; + } - if ((_ruleBook.hasChangeOccurred() && _lastSendTime + _minSendInterval < now) || _lastSendTime + _maxSendInterval < now) { - IdGrp msg; - _deepestNode = _rootNode; - _treeDepth = 0; - _rootNode->toMessage(msg, _deepestNode, _treeDepth, 0); - _teamObserver.doBroadCast(msg); - _lastSendTime = now; - _ruleBook.resetChangeOccurred(); - } + if ((_ruleBook.hasChangeOccurred() && _lastSendTime + _minSendInterval < now) || _lastSendTime + _maxSendInterval < now) { + IdGrp msg; + _deepestNode = _rootNode; + _treeDepth = 0; + _rootNode->toMessage(msg, _deepestNode, _treeDepth, 0); + _teamObserver.doBroadCast(msg); + _lastSendTime = now; + _ruleBook.resetChangeOccurred(); + } - if (_sendStatusMessages && _lastSentStatusTime + _sendStatusInterval < _clock.now()) { - if (_deepestNode != nullptr) { - _statusMessage->robotIDsWithMe.clear(); - _statusMessage->currentPlan = _deepestNode->getActivePlan()->getName(); - if (_deepestNode->getActiveEntryPoint() != nullptr) { - _statusMessage->currentTask = _deepestNode->getActiveEntryPoint()->getTask()->getName(); - } else { - _statusMessage->currentTask = "IDLE"; - } - if (_deepestNode->getActiveState() != nullptr) { - _statusMessage->currentState = _deepestNode->getActiveState()->getName(); - _deepestNode->getAssignment().getAgentsInState(_deepestNode->getActiveState(), _statusMessage->robotIDsWithMe); - } else { - _statusMessage->currentState = "NONE"; - } - auto tmpRole = _roleAssignment.getOwnRole(); - if (tmpRole) { - _statusMessage->currentRole = _roleAssignment.getOwnRole()->getName(); - } else { - _statusMessage->currentRole = "No Role"; - } - _communicator.sendAlicaEngineInfo(*_statusMessage); - _lastSentStatusTime = _clock.now(); + if (_sendStatusMessages && _lastSentStatusTime + _sendStatusInterval < _clock.now()) { + if (_deepestNode != nullptr) { + _statusMessage->robotIDsWithMe.clear(); + _statusMessage->currentPlan = _deepestNode->getActivePlan()->getName(); + if (_deepestNode->getActiveEntryPoint() != nullptr) { + _statusMessage->currentTask = _deepestNode->getActiveEntryPoint()->getTask()->getName(); + } else { + _statusMessage->currentTask = "IDLE"; + } + if (_deepestNode->getActiveState() != nullptr) { + _statusMessage->currentState = _deepestNode->getActiveState()->getName(); + _deepestNode->getAssignment().getAgentsInState(_deepestNode->getActiveState(), _statusMessage->robotIDsWithMe); + } else { + _statusMessage->currentState = "NONE"; } + auto tmpRole = _roleAssignment.getOwnRole(); + if (tmpRole) { + _statusMessage->currentRole = _roleAssignment.getOwnRole()->getName(); + } else { + _statusMessage->currentRole = "No Role"; + } + _communicator.sendAlicaEngineInfo(*_statusMessage); + _lastSentStatusTime = _clock.now(); } + } - //_ae->iterationComplete(); TODO modify when AlicaEngine::iterationComplete will be written + //_ae->iterationComplete(); TODO modify when AlicaEngine::iterationComplete will be written - now = _clock.now(); + now = _clock.now(); - AlicaTime availTime = _loopTime - (now - beginTime); - bool checkFp = false; - if (availTime > AlicaTime::milliseconds(1)) { - std::unique_lock lock(_lomutex); - checkFp = std::cv_status::no_timeout == _fpEventWait.wait_for(lock, std::chrono::nanoseconds(availTime.inNanoseconds())); + AlicaTime availTime = _loopTime - (now - beginTime); + bool checkFp = false; + if (availTime > AlicaTime::milliseconds(1)) { + std::unique_lock lock(_lomutex); + checkFp = std::cv_status::no_timeout == _fpEventWait.wait_for(lock, std::chrono::nanoseconds(availTime.inNanoseconds())); + } + + if (checkFp) { + std::queue nextFpEvents; + { + // move fast path events to a local variable. Prevents calling visit() on a RunningPlan which can add a fast path event double locking + // _loMutex + std::lock_guard lock(_lomutex); + nextFpEvents.swap(_fpEvents); } - if (checkFp) { - std::queue nextFpEvents; - { - // move fast path events to a local variable. Prevents calling visit() on a RunningPlan which can add a fast path event double locking - // _loMutex - std::lock_guard lock(_lomutex); - nextFpEvents.swap(_fpEvents); - } + while (_running && availTime > AlicaTime::milliseconds(1) && !nextFpEvents.empty()) { + RunningPlan* rp = nextFpEvents.front(); + nextFpEvents.pop(); - while (_running && availTime > AlicaTime::milliseconds(1) && !nextFpEvents.empty()) { - RunningPlan* rp = nextFpEvents.front(); - nextFpEvents.pop(); - - if (rp->isActive()) { - bool first = true; - while (rp != nullptr) { - PlanChange change = _ruleBook.visit(*rp); - if (!first && change == PlanChange::NoChange) { - break; - } - rp = rp->getParent(); - first = false; + if (rp->isActive()) { + bool first = true; + while (rp != nullptr) { + PlanChange change = _ruleBook.visit(*rp); + if (!first && change == PlanChange::NoChange) { + break; } + rp = rp->getParent(); + first = false; } - now = _clock.now(); - availTime = _loopTime - (now - beginTime); } + now = _clock.now(); + availTime = _loopTime - (now - beginTime); + } - { - // if all fast path events could not be processed, prepend them back to _fpEvents - if (!nextFpEvents.empty()) { - std::lock_guard lock(_lomutex); - _fpEvents.swap(nextFpEvents); - while (!nextFpEvents.empty()) { - _fpEvents.push(nextFpEvents.front()); - nextFpEvents.pop(); - } + { + // if all fast path events could not be processed, prepend them back to _fpEvents + if (!nextFpEvents.empty()) { + std::lock_guard lock(_lomutex); + _fpEvents.swap(nextFpEvents); + while (!nextFpEvents.empty()) { + _fpEvents.push(nextFpEvents.front()); + nextFpEvents.pop(); } } } + } - now = _clock.now(); - availTime = _loopTime - (now - beginTime); + now = _clock.now(); + availTime = _loopTime - (now - beginTime); - if (_running && availTime > AlicaTime::microseconds(100) && !_stepEngine) { - _clock.sleep(availTime); - } + if (_running && availTime > AlicaTime::microseconds(100) && !_stepEngine) { + _clock.sleep(availTime); } } @@ -332,9 +328,8 @@ void PlanBase::stop() _stepModeCV.notify_one(); } - if (_mainThread != nullptr) { - _mainThread->join(); - _mainThread.reset(); + if (_engineTimer) { + _engineTimer.reset(); } if (_rootNode) { diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/CMakeLists.txt b/supplementary/alica_ros1/alica_ros_turtlesim/CMakeLists.txt index 4f7380132..84517db2a 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/CMakeLists.txt +++ b/supplementary/alica_ros1/alica_ros_turtlesim/CMakeLists.txt @@ -73,6 +73,7 @@ add_subdirectory(libalica-ros-utils) if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest_gtest(${PROJECT_NAME}-test test/alica_ros_turtlesim.test test/test_main.cpp) + add_rostest(test/alica_ros_turtlesim_driven.test) target_link_libraries(${PROJECT_NAME}-test Boost::system Boost::filesystem diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/include/alica_ros_turtlesim/base.hpp b/supplementary/alica_ros1/alica_ros_turtlesim/include/alica_ros_turtlesim/base.hpp index eba403860..cd8201226 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/include/alica_ros_turtlesim/base.hpp +++ b/supplementary/alica_ros1/alica_ros_turtlesim/include/alica_ros_turtlesim/base.hpp @@ -1,7 +1,10 @@ #pragma once #include +#include +#include +#include #include #include @@ -16,13 +19,17 @@ class Base { public: Base(ros::NodeHandle& nh, ros::NodeHandle& priv_nh, const std::string& name, const int agent_id, const std::string& roleset, const std::string& master_plan, - const std::vector& paths, std::optional placeholderMapping = std::nullopt); + const std::vector& paths, std::optional placeholderMapping = std::nullopt, bool drivenExecutor = false); ~Base(); void start(); + void tick(); private: ros::AsyncSpinner spinner; std::unique_ptr ac; + std::shared_ptr timer_factory; + std::optional callback_queue; + bool _drivenExecutor = false; }; } // namespace turtlesim diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/launch/turtle.launch b/supplementary/alica_ros1/alica_ros_turtlesim/launch/turtle.launch index 7ccd266dd..9c45c511b 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/launch/turtle.launch +++ b/supplementary/alica_ros1/alica_ros_turtlesim/launch/turtle.launch @@ -12,6 +12,8 @@ + + @@ -20,6 +22,7 @@ + @@ -28,6 +31,7 @@ + diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/src/base.cpp b/supplementary/alica_ros1/alica_ros_turtlesim/src/base.cpp index 9940ce8ba..93d8d07bf 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/src/base.cpp +++ b/supplementary/alica_ros1/alica_ros_turtlesim/src/base.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -20,14 +21,23 @@ namespace turtlesim { Base::Base(ros::NodeHandle& nh, ros::NodeHandle& privNh, const std::string& name, const int agent_id, const std::string& roleset, - const std::string& master_plan, const std::vector& paths, std::optional placeholderMapping) + const std::string& master_plan, const std::vector& paths, std::optional placeholderMapping, bool drivenExecutor) : spinner(0) + , _drivenExecutor(drivenExecutor) { // Initialize Alica ac = std::make_unique(AlicaContextParams(name, paths, roleset, master_plan, false, agent_id, placeholderMapping)); ac->setCommunicator(); ac->setTimerFactory(); + + if (_drivenExecutor) { + callback_queue.emplace(); + ac->setEngineTimerFactory(*callback_queue); + } else { + ac->setEngineTimerFactory(); + } + ac->setLogger(); LockedBlackboardRW bb(ac->editGlobalBlackboard()); @@ -46,6 +56,13 @@ void Base::start() ac->addSolver(ac->getConfig()); } +void Base::tick() +{ + if (_drivenExecutor && callback_queue.has_value()) { + callback_queue->callAvailable(ros::WallDuration(0)); + } +} + Base::~Base() { spinner.stop(); // stop spinner before terminating engine diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/src/base_node.cpp b/supplementary/alica_ros1/alica_ros_turtlesim/src/base_node.cpp index 24a9207f8..231312f1b 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/src/base_node.cpp +++ b/supplementary/alica_ros1/alica_ros_turtlesim/src/base_node.cpp @@ -27,6 +27,7 @@ int main(int argc, char** argv) ROS_INFO("Started Turtle Base Node."); std::string name, roleset, masterPlan, turtlesimRosPath, turtlesimLibPath, placeholderMappingFilePath; int agentId; + bool drivenExecutor = false; ros::NodeHandle nh, privNh("~"); privNh.getParam("name", name); @@ -36,6 +37,7 @@ int main(int argc, char** argv) privNh.getParam("turtlesim_ros_path", turtlesimRosPath); privNh.getParam("turtlesim_lib_path", turtlesimLibPath); privNh.getParam("agent_id", agentId); + privNh.getParam("driven_executor", drivenExecutor); ROS_INFO_STREAM("HostName : " << name); ROS_INFO_STREAM("Roleset : " << (roleset.empty() ? "Default" : roleset)); @@ -44,6 +46,7 @@ int main(int argc, char** argv) ROS_INFO_STREAM("ROS Turtlesim lib path : " << turtlesimRosPath); ROS_INFO_STREAM("Base Turtlesim lib path : " << turtlesimLibPath); ROS_INFO_STREAM("Agent ID : " << agentId); + ROS_INFO_STREAM("Driven Executor : " << (drivenExecutor ? "true" : "false")); if (masterPlan.size() == 0) { ROS_ERROR_STREAM("Master plan or roleset location is not available"); @@ -65,12 +68,15 @@ int main(int argc, char** argv) return 1; } } - turtlesim::Base base(nh, privNh, name, agentId, roleset, masterPlan, {turtlesimRosPath, turtlesimLibPath}, maybeMapping); + turtlesim::Base base(nh, privNh, name, agentId, roleset, masterPlan, {turtlesimRosPath, turtlesimLibPath}, maybeMapping, drivenExecutor); ROS_INFO("Starting ALICA turtle Base......."); base.start(); while (ros::ok()) { + if (drivenExecutor) { + base.tick(); + } ros::Rate(10).sleep(); } diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim.test b/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim.test index a198aa680..8d80a5e29 100644 --- a/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim.test +++ b/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim.test @@ -6,6 +6,7 @@ + @@ -17,6 +18,7 @@ + @@ -27,6 +29,7 @@ + @@ -37,6 +40,7 @@ + @@ -47,6 +51,7 @@ + diff --git a/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim_driven.test b/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim_driven.test new file mode 100644 index 000000000..9b8890340 --- /dev/null +++ b/supplementary/alica_ros1/alica_ros_turtlesim/test/alica_ros_turtlesim_driven.test @@ -0,0 +1,6 @@ + + + + + + diff --git a/supplementary/alica_ros1/supplementary_tests/include/test_supplementary.h b/supplementary/alica_ros1/supplementary_tests/include/test_supplementary.h index 483f72f34..b110d09bf 100644 --- a/supplementary/alica_ros1/supplementary_tests/include/test_supplementary.h +++ b/supplementary/alica_ros1/supplementary_tests/include/test_supplementary.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ class AlicaTestFixture : public AlicaTestFixtureBase spinner = std::make_unique(4); ac->setCommunicator(); ac->setTimerFactory(); + ac->setEngineTimerFactory(); creators = {std::make_unique(), std::make_unique(), std::make_unique(), std::make_unique(), std::make_unique(), std::make_unique()}; @@ -140,6 +142,7 @@ class AlicaTestMultiAgentFixture : public AlicaTestMultiAgentFixtureBase ASSERT_TRUE(ac->isValid()); ac->setCommunicator(*cbQueues.back()); ac->setTimerFactory(*cbQueues.back()); + ac->setEngineTimerFactory(); EXPECT_EQ(0, ac->init(std::move(creators), true)); alica::AlicaEngine* ae = AlicaTestsEngineGetter::getEngine(ac); const_cast(ae->getCommunicator()).startCommunication(); diff --git a/supplementary/alica_ros1/supplementary_tests/src/test/test_alica_problem_composition.cpp b/supplementary/alica_ros1/supplementary_tests/src/test/test_alica_problem_composition.cpp index 8903ed6cd..f365c985d 100644 --- a/supplementary/alica_ros1/supplementary_tests/src/test/test_alica_problem_composition.cpp +++ b/supplementary/alica_ros1/supplementary_tests/src/test/test_alica_problem_composition.cpp @@ -30,7 +30,7 @@ TEST_F(AlicaProblemCompositionTest, SimpleStaticComposition) ae->start(); - STEP_UNTIL(ae->getPlanBase().getDeepestNode() == nullptr); + STEP_UNTIL(ac, ae->getPlanBase().getDeepestNode() == nullptr); const alica::RunningPlan* deep = ae->getPlanBase().getDeepestNode();