Changes have been pushed for the repository "fawkesrobotics/fawkes".
Clone: https://github.com/fawkesrobotics/fawkes.git Gitweb: https://github.com/fawkesrobotics/fawkes The branch, vmatare/gologpp-update has been updated discards 0a6f422f3069a6fed538c0211f55dea9f75708c0 (commit) discards f7aa687829120b580b4d23008aa9f265b8e8b779 (commit) discards 2469be4950eab2143924eabbbbe811baebd1fe22 (commit) discards 017f6c073d81ddeac3513fdef73deb152154b5a9 (commit) discards f9ef590c6a3225c8a182717d9e89baafe6dbd703 (commit) discards a6fa7c14e12d3a8d78e13b4722a02c94d90649d8 (commit) discards adb4fca2893823d68ecb850f843782c5ccb4b348 (commit) discards a85e3d1e80838b152bae2def40d6d13ef4fa8257 (commit) discards 0bd58d29566234a4629b2e74dd59b7b0ef81743b (commit) to 2175ef6c5c115a2e2c3a1a3b5b168912cab25240 (commit) via f12f024147c528425c3a5b93538da484f9cd284a (commit) via 817b6417a7c968349056c87ff2f366dbc59c6490 (commit) via 563236be9c67a7345a5a0a679dde8ed9ad107e60 (commit) via 9c3534d43e6e3737809841fddd03a5cea2afc895 (commit) via 334e6541a88c92b8b47ef3ad74a4d55c4da40527 (commit) via f55b1db4e6b659a391684bcd0b4b5808e2806286 (commit) via e3b5fcc8ef03cdf6a3aed59c0156a9d1278f7024 (commit) via d3e78911c98b69feba38ab7ca6b4fc0ede015600 (commit) via 1094a27c45f6146415e2bf57dcd9eadc4747f468 (commit) via 87c7705881a59784ca1fb5aaa384bf2ea90a6e22 (commit) via 003526a0db8dffa6e711418d1fff5f6645272394 (commit) via a0b67aaba41a1bdaf4023decdd85eb0ac4d86a8b (commit) This update added new revisions after undoing existing revisions. That is to say, the old revision is not a strict subset of the new revision. This situation occurs when you --force push a change and generate a repository containing something like this: * -- * -- B -- O -- O -- O (0a6f422f3069a6fed538c0211f55dea9f75708c0) \ N -- N -- N (2175ef6c5c115a2e2c3a1a3b5b168912cab25240) When this happens we assume that you've already had alert emails for all of the O revisions, and so we here report only the revisions in the N branch from the common base, B. https://github.com/fawkesrobotics/fawkes/tree/vmatare/gologpp-update Those revisions listed above that are new to this repository have not appeared on any other notification email; so we list those revisions in full, below. - *Log* --------------------------------------------------------------- commit d3e78911c98b69feba38ab7ca6b4fc0ede015600 Author: Victor Mataré <[email protected]> AuthorDate: Fri Oct 16 13:33:08 2020 +0200 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: adapt to API changes https://github.com/fawkesrobotics/fawkes/commit/d3e78911c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit e3b5fcc8ef03cdf6a3aed59c0156a9d1278f7024 Author: Victor Mataré <[email protected]> AuthorDate: Tue Oct 20 18:13:23 2020 +0200 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: throw on missing main() procedure https://github.com/fawkesrobotics/fawkes/commit/e3b5fcc8e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit f55b1db4e6b659a391684bcd0b4b5808e2806286 Author: Victor Mataré <[email protected]> AuthorDate: Tue Oct 20 19:01:27 2020 +0200 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: log missing exog action field mappings https://github.com/fawkesrobotics/fawkes/commit/f55b1db4e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 334e6541a88c92b8b47ef3ad74a4d55c4da40527 Author: Victor Mataré <[email protected]> AuthorDate: Wed Nov 4 00:55:17 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: support setting golog++ loglevel via config https://github.com/fawkesrobotics/fawkes/commit/334e6541a - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 9c3534d43e6e3737809841fddd03a5cea2afc895 Author: Victor Mataré <[email protected]> AuthorDate: Wed Nov 4 00:58:25 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: implement some nice exogenous functions So golog++ code can now transform poses, convert quaternions and query the config. W00t. https://github.com/fawkesrobotics/fawkes/commit/9c3534d43 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 563236be9c67a7345a5a0a679dde8ed9ad107e60 Author: Victor Mataré <[email protected]> AuthorDate: Sat Nov 7 16:40:09 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: move skiller interaction into separate class We need this because some component backends may want to use the skiller, too, and we can only have one interface reader with exclusive control. https://github.com/fawkesrobotics/fawkes/commit/563236be9 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 817b6417a7c968349056c87ff2f366dbc59c6490 Author: Victor Mataré <[email protected]> AuthorDate: Tue Nov 10 03:06:42 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: fix non-virtual d'tor warning https://github.com/fawkesrobotics/fawkes/commit/817b6417a - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit f12f024147c528425c3a5b93538da484f9cd284a Author: Victor Mataré <[email protected]> AuthorDate: Sat Nov 14 16:18:38 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: prevent premature exog event crash https://github.com/fawkesrobotics/fawkes/commit/f12f02414 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 2175ef6c5c115a2e2c3a1a3b5b168912cab25240 Author: Victor Mataré <[email protected]> AuthorDate: Sat Nov 14 16:19:56 2020 +0100 Commit: Victor Mataré <[email protected]> CommitDate: Tue Nov 17 01:45:22 2020 +0100 gologpp: catch gologpp::EngineError https://github.com/fawkesrobotics/fawkes/commit/2175ef6c5 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- src/libs/blackboard/interface_listener.cpp | 8 ++++++-- src/libs/blackboard/net/handler.cpp | 19 ++++++++++++------- src/libs/blackboard/net/interface_listener.cpp | 21 +++++++++++++++++++++ src/libs/blackboard/net/interface_listener.h | 1 + src/libs/blackboard/net/interface_proxy.cpp | 2 +- src/libs/blackboard/net/interface_proxy.h | 2 +- src/libs/blackboard/remote.cpp | 4 ++-- src/libs/interface/interface.cpp | 17 ++++++++++++++--- src/libs/interface/interface.h | 1 + 9 files changed, 59 insertions(+), 16 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* d3e78911c98b69feba38ab7ca6b4fc0ede015600 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Fri Oct 16 13:33:08 2020 +0200 Subject: gologpp: adapt to API changes src/plugins/gologpp/action_executor.cpp | 3 ++ src/plugins/gologpp/action_executor.h | 3 +- .../gologpp/aspect/action_executor_dispatcher.cpp | 11 +++++- .../gologpp/aspect/action_executor_dispatcher.h | 1 + src/plugins/gologpp/execution_thread.cpp | 8 ++-- src/plugins/gologpp/execution_thread.h | 14 +++---- src/plugins/gologpp/exog_manager.cpp | 46 +++++++++------------- src/plugins/gologpp/exog_manager.h | 9 ++--- src/plugins/gologpp/gologpp_fawkes_backend.cpp | 12 +++++- src/plugins/gologpp/gologpp_fawkes_backend.h | 5 ++- src/plugins/gologpp/message_action_executor.cpp | 10 ++++- src/plugins/gologpp/message_action_executor.h | 3 +- src/plugins/gologpp/print_action_executor.cpp | 10 ++++- src/plugins/gologpp/print_action_executor.h | 5 +-- src/plugins/gologpp/remote_skiller_executor.cpp | 3 +- src/plugins/gologpp/skiller_action_executor.cpp | 15 +++++-- src/plugins/gologpp/skiller_action_executor.h | 5 ++- src/plugins/gologpp/sleep_action_executor.cpp | 10 +++-- src/plugins/gologpp/sleep_action_executor.h | 3 +- 19 files changed, 109 insertions(+), 67 deletions(-) _Diff for modified files_: diff --git a/src/plugins/gologpp/action_executor.cpp b/src/plugins/gologpp/action_executor.cpp index b024314f9..2cfc10340 100644 --- a/src/plugins/gologpp/action_executor.cpp +++ b/src/plugins/gologpp/action_executor.cpp @@ -45,6 +45,9 @@ namespace gpp { * @param activity The activity to execute. * @return True iff the given activity can be executed by this executor. * + * @fn ActionExecutor::terminate() + * Stop running action(s) + * * @var ActionExecutor::running_activity_ * A pointer to the currently running activity. * diff --git a/src/plugins/gologpp/action_executor.h b/src/plugins/gologpp/action_executor.h index 5a0524585..f9abe9b4d 100644 --- a/src/plugins/gologpp/action_executor.h +++ b/src/plugins/gologpp/action_executor.h @@ -36,8 +36,9 @@ class ActionExecutor public: ActionExecutor(Logger *logger); virtual void start(std::shared_ptr<gologpp::Activity> activity) = 0; - virtual void stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) = 0; + virtual void stop(std::shared_ptr<gologpp::Activity> activity) = 0; virtual bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const = 0; + virtual void terminate() = 0; protected: std::shared_ptr<gologpp::Activity> running_activity_; diff --git a/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp b/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp index 9ae3b7f2a..c3dca7d59 100644 --- a/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp +++ b/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp @@ -21,7 +21,7 @@ #include "action_executor_dispatcher.h" #include <core/exception.h> -#include <golog++/model/activity.h> +#include <golog++/execution/activity.h> namespace fawkes { namespace gpp { @@ -59,6 +59,15 @@ ActionExecutorDispatcher::register_executor(std::shared_ptr<ActionExecutor> exec action_executors_.push_back(executor); } +/** Terminate all executors + */ +void +ActionExecutorDispatcher::terminate() +{ + for (auto &executor : action_executors_) + executor->terminate(); +} + } // namespace gpp /** @class GologppDispatcherAspect diff --git a/src/plugins/gologpp/aspect/action_executor_dispatcher.h b/src/plugins/gologpp/aspect/action_executor_dispatcher.h index 7e60e4d07..71aeb13d0 100644 --- a/src/plugins/gologpp/aspect/action_executor_dispatcher.h +++ b/src/plugins/gologpp/aspect/action_executor_dispatcher.h @@ -33,6 +33,7 @@ class ActionExecutorDispatcher public: std::shared_ptr<ActionExecutor> get_executor(std::shared_ptr<gologpp::Activity>); void register_executor(std::shared_ptr<ActionExecutor> executor); + void terminate(); private: std::vector<std::shared_ptr<ActionExecutor>> action_executors_; diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index 409851cf5..614e189b3 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -69,7 +69,8 @@ GologppThread::init() new ListType(*global_scope().lookup_type(SymbolType::static_name()))); logger->log_info(name(), "Parsing %s...", prog_file.c_str()); - main_prog_ = gologpp::parser::parse_file(prog_file); + gologpp::parser::parse_file(prog_file); + main_prog_ = gologpp::global_scope().lookup_global<Procedure>("main"); logger->log_info(name(), "... parsing done"); logger->log_info(name(), "Initializing ReadyLog context..."); @@ -90,8 +91,7 @@ GologppThread::once() { try { std::lock_guard<std::mutex> l{run_mutex_}; - gologpp::ReadylogContext::instance().run( - gologpp::Block{new gologpp::Scope{gologpp::global_scope()}, {main_prog_.release()}}); + gologpp::ReadylogContext::instance().run(main_prog_->definition()); logger->log_info(name(), "golog++ main program has ended"); } catch (gologpp::UserError &e) { logger->log_error(name(), "User Error: %s", e.what()); @@ -121,7 +121,7 @@ GologppThread::finalize() * @brief GologppThread::gologpp_context * @return the currently used golog++ execution context. */ -gologpp::ExecutionContext & +gologpp::ExecutionController & GologppThread::gologpp_context() { return gologpp::ReadylogContext::instance(); diff --git a/src/plugins/gologpp/execution_thread.h b/src/plugins/gologpp/execution_thread.h index dcd1f62c2..f6e50b004 100644 --- a/src/plugins/gologpp/execution_thread.h +++ b/src/plugins/gologpp/execution_thread.h @@ -26,7 +26,7 @@ #include <aspect/logging.h> #include <blackboard/interface_listener.h> #include <core/threading/thread.h> -#include <golog++/model/execution.h> +#include <golog++/execution/controller.h> #include <filesystem> #include <string> @@ -55,14 +55,14 @@ public: virtual void finalize() override; - gologpp::ExecutionContext &gologpp_context(); + gologpp::ExecutionController &gologpp_context(); private: - std::filesystem::path find_prog_file(const std::filesystem::path &spec) const; - std::unique_ptr<gologpp::Instruction> main_prog_; - SkillerInterface * skiller_if_; - ExogManager * exog_mgr_; - std::mutex run_mutex_; + std::filesystem::path find_prog_file(const std::filesystem::path &spec) const; + std::shared_ptr<gologpp::Procedure> main_prog_; + SkillerInterface * skiller_if_; + ExogManager * exog_mgr_; + std::mutex run_mutex_; }; } // namespace gpp diff --git a/src/plugins/gologpp/exog_manager.cpp b/src/plugins/gologpp/exog_manager.cpp index afc2504a1..87ec690ad 100644 --- a/src/plugins/gologpp/exog_manager.cpp +++ b/src/plugins/gologpp/exog_manager.cpp @@ -24,7 +24,8 @@ #include "utils.h" #include <core/exception.h> -#include <golog++/model/grounding.h> +#include <golog++/model/mapping.h> +#include <golog++/model/reference.h> #include <libs/interface/field_iterator.h> using namespace fawkes; @@ -189,11 +190,6 @@ ExogManager::BlackboardEventHandler::BlackboardEventHandler( + desired_type->name()); } blackboard_->close(iface); - - auto param_it = - std::find(target_exog_->params().begin(), target_exog_->params().end(), var_ref.target()); - auto param_idx = param_it - target_exog_->params().begin(); - fields_order_.emplace(pair.first, arity_t(param_idx)); } } @@ -236,38 +232,34 @@ ExogManager::InterfaceWatcher::~InterfaceWatcher() shared_ptr<ExogEvent> ExogManager::BlackboardEventHandler::make_exog_event(Interface *iface) const { - // clang-format off - // alignment of assignments just makes this unreadable iface->read(); InterfaceFieldIterator fi = iface->fields(); - vector<unique_ptr<Value>> args(target_exog_->arity()); + Binding binding; while (fi != iface->fields_end()) { - if (target_exog_->mapping().is_mapped(fi.get_name())) { - auto order_it = fields_order_.find(fi.get_name()); - + shared_ptr<const Variable> mapped_var = target_exog_->mapping().mapped_var(fi.get_name()); + if (mapped_var) { if (fi.get_length() == 1 || (fi.get_length() > 1 && fi.get_type() == IFT_STRING)) - args[order_it->second].reset(field_to_value(fi, 0)); + binding.bind(mapped_var, gologpp::unique_ptr<Expression>(field_to_value(fi, 0))); else if (fi.get_length() > 1) { - vector<unique_ptr<Value>> list_init; + vector<unique_ptr<Value>> list_value; for (unsigned int idx = 0; idx < fi.get_length(); ++idx) - list_init.emplace_back( - field_to_value(fi, idx) - ); - shared_ptr<const Type> list_type = global_scope().lookup_list_type(list_init[0]->type()); - args[order_it->second].reset( - new Value(*list_type, list_init) - ); - } - else - throw IllegalArgumentException("%s: Field %s has length %d and type %s, which shouldn't happen", - iface->uid(), fi.get_name(), fi.get_length(), fi.get_typename()); + list_value.emplace_back(field_to_value(fi, idx)); + shared_ptr<const Type> list_type = global_scope().lookup_list_type(list_value[0]->type()); + binding.bind(mapped_var, + gologpp::unique_ptr<Expression>(new Value(*list_type, list_value))); + } else + throw IllegalArgumentException( + "%s: Field %s has length %d and type %s, which shouldn't happen", + iface->uid(), + fi.get_name(), + fi.get_length(), + fi.get_typename()); } ++fi; } - return std::make_shared<ExogEvent>(target_exog_, std::move(args)); - // clang-format on + return gologpp::shared_ptr<ExogEvent>(new ExogEvent(target_exog_, std::move(binding))); } void diff --git a/src/plugins/gologpp/exog_manager.h b/src/plugins/gologpp/exog_manager.h index 8741793e2..89c4536a8 100644 --- a/src/plugins/gologpp/exog_manager.h +++ b/src/plugins/gologpp/exog_manager.h @@ -26,8 +26,8 @@ #include <blackboard/interface_observer.h> #include <config/config.h> #include <core/threading/thread.h> +#include <golog++/execution/controller.h> #include <golog++/model/action.h> -#include <golog++/model/execution.h> #include <logging/logger.h> namespace gologpp { @@ -79,10 +79,9 @@ private: static std::string extract_id(const std::string &iface_uid); protected: - BlackBoard * blackboard_; - gologpp::shared_ptr<gologpp::ExogAction> target_exog_; - std::unordered_map<std::string, gologpp::arity_t> fields_order_; - ExogManager & exog_manager_; + BlackBoard * blackboard_; + gologpp::shared_ptr<gologpp::ExogAction> target_exog_; + ExogManager & exog_manager_; }; /////////////////////////////////////////////////////////////////// diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.cpp b/src/plugins/gologpp/gologpp_fawkes_backend.cpp index 6edd7f5c1..603e44591 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.cpp +++ b/src/plugins/gologpp/gologpp_fawkes_backend.cpp @@ -28,8 +28,8 @@ #include "sleep_action_executor.h" #include <config/config.h> -#include <golog++/model/activity.h> -#include <golog++/model/transition.h> +#include <golog++/execution/activity.h> +#include <golog++/execution/transition.h> namespace fawkes { namespace gpp { @@ -102,6 +102,14 @@ GologppFawkesBackend::time() const noexcept gologpp::Clock::duration{clock->now().in_sec() / gologpp::Clock::duration::period::den}}; } +/** Terminate execution of running actions + */ +void +GologppFawkesBackend::terminate_() +{ + action_dispatcher_.terminate(); +} + /** Execute the given activity using a suitable executor. * @param a The activity to start. */ diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.h b/src/plugins/gologpp/gologpp_fawkes_backend.h index 900a40f41..8d0bd956d 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.h +++ b/src/plugins/gologpp/gologpp_fawkes_backend.h @@ -30,7 +30,7 @@ #include <aspect/clock.h> #include <aspect/inifins/inifin.h> #include <blackboard/blackboard.h> -#include <golog++/model/platform_backend.h> +#include <golog++/execution/platform_backend.h> #include <logging/logger.h> namespace fawkes { @@ -51,10 +51,11 @@ public: BlackBoard * blackboard); virtual ~GologppFawkesBackend(); - virtual void preempt_activity(std::shared_ptr<gologpp::Activity> a) override; virtual gologpp::Clock::time_point time() const noexcept override; private: + virtual void terminate_() override; + virtual void preempt_activity(std::shared_ptr<gologpp::Activity> a) override; virtual void execute_activity(std::shared_ptr<gologpp::Activity>) override; SkillerInterface * skiller_if_; diff --git a/src/plugins/gologpp/message_action_executor.cpp b/src/plugins/gologpp/message_action_executor.cpp index 25a0d3980..efd507334 100644 --- a/src/plugins/gologpp/message_action_executor.cpp +++ b/src/plugins/gologpp/message_action_executor.cpp @@ -24,7 +24,8 @@ #include <blackboard/blackboard.h> #include <config/config.h> -#include <golog++/model/activity.h> +#include <golog++/execution/activity.h> +#include <golog++/model/mapping.h> #include <logging/logger.h> #include <algorithm> @@ -69,6 +70,11 @@ BBMessageActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> } void +BBMessageActionExecutor::terminate() +{ +} + +void BBMessageActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { @@ -110,7 +116,7 @@ BBMessageActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) } void -BBMessageActionExecutor::stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) +BBMessageActionExecutor::stop(std::shared_ptr<gologpp::Activity> activity) { logger_->log_error("BBMessageActionExecutor", "Cannot stop a message that has already been sent!"); diff --git a/src/plugins/gologpp/message_action_executor.h b/src/plugins/gologpp/message_action_executor.h index e3d5b1c5b..f71079b11 100644 --- a/src/plugins/gologpp/message_action_executor.h +++ b/src/plugins/gologpp/message_action_executor.h @@ -42,8 +42,9 @@ public: const std::string &cfg_prefix); virtual ~BBMessageActionExecutor(); void start(std::shared_ptr<gologpp::Activity> activity) override; - void stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) override; + void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; + void terminate() override; private: BlackBoard * blackboard_; diff --git a/src/plugins/gologpp/print_action_executor.cpp b/src/plugins/gologpp/print_action_executor.cpp index 7b2537e83..d2bb4983f 100644 --- a/src/plugins/gologpp/print_action_executor.cpp +++ b/src/plugins/gologpp/print_action_executor.cpp @@ -22,6 +22,8 @@ #include "utils.h" +#include <golog++/execution/activity.h> +#include <golog++/model/mapping.h> #include <logging/logger.h> namespace fawkes { @@ -51,6 +53,11 @@ PrintActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> act } void +PrintActionExecutor::terminate() +{ +} + +void PrintActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { @@ -78,8 +85,7 @@ PrintActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) activity->update(gologpp::Transition::Hook::FINISH); } -void -PrintActionExecutor::stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) +void PrintActionExecutor::stop(std::shared_ptr<gologpp::Activity>) { logger_->log_error("PrintActionExecutor", "Cannot stop printing a message!"); } diff --git a/src/plugins/gologpp/print_action_executor.h b/src/plugins/gologpp/print_action_executor.h index d1be1dff5..4c225adec 100644 --- a/src/plugins/gologpp/print_action_executor.h +++ b/src/plugins/gologpp/print_action_executor.h @@ -22,8 +22,6 @@ #include "action_executor.h" -#include <golog++/model/activity.h> - namespace fawkes { class Logger; @@ -36,8 +34,9 @@ public: virtual ~PrintActionExecutor(); void start(std::shared_ptr<gologpp::Activity> activity) override; - void stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) override; + void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; + void terminate() override; }; } // namespace gpp } // namespace fawkes diff --git a/src/plugins/gologpp/remote_skiller_executor.cpp b/src/plugins/gologpp/remote_skiller_executor.cpp index 95dd538d6..63b2e301b 100644 --- a/src/plugins/gologpp/remote_skiller_executor.cpp +++ b/src/plugins/gologpp/remote_skiller_executor.cpp @@ -21,7 +21,8 @@ #include "remote_skiller_executor.h" #include <blackboard/remote.h> -#include <golog++/model/activity.h> +#include <golog++/execution/activity.h> +#include <golog++/model/mapping.h> namespace fawkes { namespace gpp { diff --git a/src/plugins/gologpp/skiller_action_executor.cpp b/src/plugins/gologpp/skiller_action_executor.cpp index 011c58cbb..e31f6575a 100644 --- a/src/plugins/gologpp/skiller_action_executor.cpp +++ b/src/plugins/gologpp/skiller_action_executor.cpp @@ -22,7 +22,8 @@ #include <blackboard/blackboard.h> #include <config/config.h> -#include <golog++/model/activity.h> +#include <golog++/execution/activity.h> +#include <golog++/model/mapping.h> #include <interfaces/SkillerInterface.h> #include <logging/logger.h> @@ -161,7 +162,7 @@ SkillerActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) * @param activity The activity to stop */ void -SkillerActionExecutor::stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) +SkillerActionExecutor::stop(std::shared_ptr<gologpp::Activity> activity) { if (*running_activity_ == *activity) { skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage()); @@ -214,7 +215,7 @@ SkillerActionExecutor::map_activity_to_skill(std::shared_ptr<gologpp::Activity> * @param iface The interface that has changed */ void -SkillerActionExecutor::bb_interface_data_refreshed(Interface *iface) throw() +SkillerActionExecutor::bb_interface_data_changed(Interface *iface) throw() { if (!running_activity_) { return; @@ -238,5 +239,13 @@ SkillerActionExecutor::bb_interface_data_refreshed(Interface *iface) throw() } } +void +SkillerActionExecutor::terminate() +{ + if (running_activity_) + skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage()); + running_activity_.reset(); +} + } // namespace gpp } // namespace fawkes diff --git a/src/plugins/gologpp/skiller_action_executor.h b/src/plugins/gologpp/skiller_action_executor.h index 00ef67bae..facad1483 100644 --- a/src/plugins/gologpp/skiller_action_executor.h +++ b/src/plugins/gologpp/skiller_action_executor.h @@ -50,9 +50,10 @@ public: const std::string &cfg_prefix); virtual ~SkillerActionExecutor() override; void start(std::shared_ptr<gologpp::Activity> activity) override; - void stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) override; + void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; - virtual void bb_interface_data_refreshed(Interface *) throw() override; + virtual void bb_interface_data_changed(Interface *) throw() override; + virtual void terminate() override; protected: const char *name() const; diff --git a/src/plugins/gologpp/sleep_action_executor.cpp b/src/plugins/gologpp/sleep_action_executor.cpp index 605987813..94fe536ba 100644 --- a/src/plugins/gologpp/sleep_action_executor.cpp +++ b/src/plugins/gologpp/sleep_action_executor.cpp @@ -20,7 +20,7 @@ #include "sleep_action_executor.h" -#include <golog++/model/activity.h> +#include <golog++/execution/activity.h> #include <logging/logger.h> #include <chrono> @@ -62,6 +62,11 @@ SleepActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> act } void +SleepActionExecutor::terminate() +{ +} + +void SleepActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { @@ -85,8 +90,7 @@ SleepActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) }); } -void -SleepActionExecutor::stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) +void SleepActionExecutor::stop(std::shared_ptr<gologpp::Activity>) { } diff --git a/src/plugins/gologpp/sleep_action_executor.h b/src/plugins/gologpp/sleep_action_executor.h index e235f0738..8dff21d9d 100644 --- a/src/plugins/gologpp/sleep_action_executor.h +++ b/src/plugins/gologpp/sleep_action_executor.h @@ -37,8 +37,9 @@ public: SleepActionExecutor(Logger *logger); virtual ~SleepActionExecutor(); void start(std::shared_ptr<gologpp::Activity> activity) override; - void stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity) override; + void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; + void terminate() override; private: std::list<std::future<void>> running_sleeps_; - *commit* e3b5fcc8ef03cdf6a3aed59c0156a9d1278f7024 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Tue Oct 20 18:13:23 2020 +0200 Subject: gologpp: throw on missing main() procedure src/plugins/gologpp/execution_thread.cpp | 2 ++ 1 file changed, 2 insertions(+) _Diff for modified files_: diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index 614e189b3..b3646f76a 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -71,6 +71,8 @@ GologppThread::init() logger->log_info(name(), "Parsing %s...", prog_file.c_str()); gologpp::parser::parse_file(prog_file); main_prog_ = gologpp::global_scope().lookup_global<Procedure>("main"); + if (!main_prog_) + throw fawkes::NullPointerException("No procedure main() in %s", prog_file.string().c_str()); logger->log_info(name(), "... parsing done"); logger->log_info(name(), "Initializing ReadyLog context..."); - *commit* f55b1db4e6b659a391684bcd0b4b5808e2806286 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Tue Oct 20 19:01:27 2020 +0200 Subject: gologpp: log missing exog action field mappings src/plugins/gologpp/exog_manager.cpp | 52 ++++++++++++++++++++++-------------- 1 file changed, 32 insertions(+), 20 deletions(-) _Diff for modified files_: diff --git a/src/plugins/gologpp/exog_manager.cpp b/src/plugins/gologpp/exog_manager.cpp index 87ec690ad..fa7b1b6d6 100644 --- a/src/plugins/gologpp/exog_manager.cpp +++ b/src/plugins/gologpp/exog_manager.cpp @@ -237,24 +237,34 @@ ExogManager::BlackboardEventHandler::make_exog_event(Interface *iface) const Binding binding; while (fi != iface->fields_end()) { - shared_ptr<const Variable> mapped_var = target_exog_->mapping().mapped_var(fi.get_name()); - if (mapped_var) { - if (fi.get_length() == 1 || (fi.get_length() > 1 && fi.get_type() == IFT_STRING)) - binding.bind(mapped_var, gologpp::unique_ptr<Expression>(field_to_value(fi, 0))); - else if (fi.get_length() > 1) { - vector<unique_ptr<Value>> list_value; - for (unsigned int idx = 0; idx < fi.get_length(); ++idx) - list_value.emplace_back(field_to_value(fi, idx)); - shared_ptr<const Type> list_type = global_scope().lookup_list_type(list_value[0]->type()); - binding.bind(mapped_var, - gologpp::unique_ptr<Expression>(new Value(*list_type, list_value))); - } else - throw IllegalArgumentException( - "%s: Field %s has length %d and type %s, which shouldn't happen", - iface->uid(), - fi.get_name(), - fi.get_length(), - fi.get_typename()); + try { + shared_ptr<const Variable> mapped_var = target_exog_->mapping().mapped_var(fi.get_name()); + if (mapped_var) { + if (fi.get_length() == 1 || (fi.get_length() > 1 && fi.get_type() == IFT_STRING)) + binding.bind(mapped_var, gologpp::unique_ptr<Expression>(field_to_value(fi, 0))); + else if (fi.get_length() > 1) { + vector<unique_ptr<Value>> list_value; + for (unsigned int idx = 0; idx < fi.get_length(); ++idx) + list_value.emplace_back(field_to_value(fi, idx)); + shared_ptr<const Type> list_type = global_scope().lookup_list_type(list_value[0]->type()); + binding.bind(mapped_var, + gologpp::unique_ptr<Expression>(new Value(*list_type, list_value))); + } else + throw IllegalArgumentException( + "Field %s has length %d and type %s, which shouldn't happen", + fi.get_name(), + fi.get_length(), + fi.get_typename()); + } else { + throw fawkes::IllegalArgumentException( + "Field %s maps to an expression that is not a variable", fi.get_name()); + } + } catch (gologpp::NameNotMapped &) { + exog_manager_.logger_->log_debug(exog_manager_.name(), + "%s event for %s: Field %s is not mapped (ignoring).", + target_exog_->name().c_str(), + iface->id(), + fi.get_name()); } ++fi; } @@ -269,11 +279,13 @@ ExogManager::InterfaceWatcher::bb_interface_data_refreshed(Interface *iface) thr exog_manager_.exog_queue_push(make_exog_event(iface)); } catch (IllegalArgumentException &e) { exog_manager_.logger_->log_error(exog_manager_.name(), - "Error when creating exogenous event: %s, ignoring event!", + "Error creating exogenous event for %s: %s, ignoring event!", + iface->id(), e.what_no_backtrace()); } catch (gologpp::UserError &e) { exog_manager_.logger_->log_error(exog_manager_.name(), - "Error when creating exogenous event: %s, ignoring event!", + "Error creating exogenous event for %s: %s, ignoring event!", + iface->id(), e.what()); } } - *commit* 334e6541a88c92b8b47ef3ad74a4d55c4da40527 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Wed Nov 4 00:55:17 2020 +0100 Subject: gologpp: support setting golog++ loglevel via config src/plugins/gologpp/execution_thread.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) _Diff for modified files_: diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index b3646f76a..d4fbfab65 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -23,6 +23,7 @@ #include "exog_manager.h" #include "gologpp_fawkes_backend.h" +#include <golog++/model/logger.h> #include <golog++/model/procedural.h> #include <golog++/parser/parser.h> #include <golog++/semantics/readylog/execution.h> @@ -68,6 +69,21 @@ GologppThread::init() global_scope().register_type_raw( new ListType(*global_scope().lookup_type(SymbolType::static_name()))); + try { + gologpp::Logger::instance().log_lvl() = + std::max(gologpp::LogLevel::ERR, + std::min(gologpp::LogLevel::DBG, + static_cast<gologpp::LogLevel>( + config->get_uint((cfg_prefix + "/loglevel").c_str())))); + logger->log_info(name(), + "golog++ using loglevel %d", + static_cast<int>(gologpp::Logger::instance().log_lvl())); + } catch (fawkes::ConfigEntryNotFoundException &) { + logger->log_info(name(), + "golog++ using default loglevel %d", + static_cast<int>(gologpp::Logger::instance().log_lvl())); + } + logger->log_info(name(), "Parsing %s...", prog_file.c_str()); gologpp::parser::parse_file(prog_file); main_prog_ = gologpp::global_scope().lookup_global<Procedure>("main"); - *commit* 9c3534d43e6e3737809841fddd03a5cea2afc895 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Wed Nov 4 00:58:25 2020 +0100 Subject: gologpp: implement some nice exogenous functions src/plugins/gologpp/Makefile | 3 + src/plugins/gologpp/execution_thread.cpp | 5 +- src/plugins/gologpp/execution_thread.h | 4 +- src/plugins/gologpp/gologpp_fawkes_backend.cpp | 106 +++++++++++++++++++++++-- src/plugins/gologpp/gologpp_fawkes_backend.h | 18 ++++- 5 files changed, 124 insertions(+), 12 deletions(-) _Diff for modified files_: diff --git a/src/plugins/gologpp/Makefile b/src/plugins/gologpp/Makefile index 0d9f27869..b76158ad7 100644 --- a/src/plugins/gologpp/Makefile +++ b/src/plugins/gologpp/Makefile @@ -14,8 +14,10 @@ BASEDIR = ../../.. include $(BASEDIR)/etc/buildsys/config.mk +include $(BUILDCONFDIR)/tf/tf.mk LIBS_gologpp = fawkescore fawkesutils fawkesblackboard \ + fawkestf \ fawkesinterface SkillerInterface OBJS_gologpp = plugin.o execution_thread.o gologpp_fawkes_backend.o exog_manager.o \ action_executor.o skiller_action_executor.o \ @@ -32,6 +34,7 @@ ifneq ($(HAVE_CPP17),) HAVE_GOLOGPP = $(if $(shell $(PKGCONFIG) --exists golog++; echo $${?/1/}),1,0) ifeq ($(HAVE_GOLOGPP),1) CFLAGS += $(shell $(PKGCONFIG) --cflags readylog++ parsegolog++) $(CFLAGS_CPP17) + CFLAGS += $(CFLAGS_TF) LDFLAGS += $(shell $(PKGCONFIG) --libs readylog++ parsegolog++) $(LDFLAGS_CPP17) PLUGINS_build = $(PLUGINS_all) else diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index d4fbfab65..9f2ca7acf 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -98,8 +98,9 @@ GologppThread::init() gologpp::eclipse_opts options; options.trace = config->get_bool_or_default((cfg_prefix + "/trace").c_str(), false); options.guitrace = options.trace; - gologpp::ReadylogContext::init( - options, std::make_unique<GologppFawkesBackend>(config, spec_cfg_prefix, logger, blackboard)); + gologpp::ReadylogContext::init(options, + std::make_unique<GologppFawkesBackend>( + config, spec_cfg_prefix, logger, blackboard, tf_listener)); logger->log_info(name(), "... initialization done"); } diff --git a/src/plugins/gologpp/execution_thread.h b/src/plugins/gologpp/execution_thread.h index f6e50b004..d8f752324 100644 --- a/src/plugins/gologpp/execution_thread.h +++ b/src/plugins/gologpp/execution_thread.h @@ -24,6 +24,7 @@ #include <aspect/blackboard.h> #include <aspect/configurable.h> #include <aspect/logging.h> +#include <aspect/tf.h> #include <blackboard/interface_listener.h> #include <core/threading/thread.h> #include <golog++/execution/controller.h> @@ -43,7 +44,8 @@ class ExogManager; class GologppThread : public Thread, public LoggingAspect, public BlackBoardAspect, - public ConfigurableAspect + public ConfigurableAspect, + public TransformAspect { public: GologppThread(); diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.cpp b/src/plugins/gologpp/gologpp_fawkes_backend.cpp index 603e44591..f527f6a31 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.cpp +++ b/src/plugins/gologpp/gologpp_fawkes_backend.cpp @@ -47,12 +47,18 @@ using namespace gologpp; * @param cfg_prefix The spec-specific config prefix to use * @param logger The logger to use for log messages * @param blackboard The blackboard to use to access the skiller + * @param tf_listener Used for some exog_function implementations */ -GologppFawkesBackend::GologppFawkesBackend(Configuration *config, - std::string cfg_prefix, - Logger * logger, - BlackBoard * blackboard) -: AspectProviderAspect(&dispatcher_inifin_), logger_(logger), blackboard_(blackboard) +GologppFawkesBackend::GologppFawkesBackend(Configuration * config, + std::string cfg_prefix, + Logger * logger, + BlackBoard * blackboard, + tf::Transformer *tf_listener) +: AspectProviderAspect(&dispatcher_inifin_), + logger_(logger), + blackboard_(blackboard), + tf_listener_(tf_listener), + config_(config) { // Register RemoteSkillerActionExecutors before the local // SkillerActionExecutor. This way, any action that cannot be executed on any @@ -102,6 +108,96 @@ GologppFawkesBackend::time() const noexcept gologpp::Clock::duration{clock->now().in_sec() / gologpp::Clock::duration::period::den}}; } +Value +GologppFawkesBackend::eval_exog_function(const Type & ret_type, + const std::string & name, + const std::unordered_map<std::string, Value> &args) +{ + const NumberType &number_type = get_type<NumberType>(); + + if (name == "fawkes_transform_pose") { + const CompoundType &pose_type = dynamic_cast<const CompoundType &>(ret_type); + + string to_frame = static_cast<string>(args.at("to_frame")); + + const auto & gpp_pose = static_cast<CompoundType::Representation>(args.at("src")); + const auto & gpp_trans = static_cast<CompoundType::Representation>(*gpp_pose.at("trans")); + const auto & gpp_rot = static_cast<CompoundType::Representation>(*gpp_pose.at("rot")); + const string &from_frame = static_cast<string>(*gpp_pose.at("frame")); + + tf::Pose pose_in({gpp_rot.at("x")->numeric_convert<tf::Scalar>(), + gpp_rot.at("y")->numeric_convert<tf::Scalar>(), + gpp_rot.at("z")->numeric_convert<tf::Scalar>(), + gpp_rot.at("w")->numeric_convert<tf::Scalar>()}, + {gpp_trans.at("x")->numeric_convert<tf::Scalar>(), + gpp_trans.at("y")->numeric_convert<tf::Scalar>(), + gpp_trans.at("z")->numeric_convert<tf::Scalar>()}); + + tf::Stamped<tf::Pose> out; + tf_listener_->transform_pose(to_frame, + tf::Stamped<tf::Pose>(pose_in, Time(0, 0), from_frame), + out); + + CompoundType::Representation trans_raw; + trans_raw.emplace("x", new Value(number_type, out.getOrigin().getX())); + trans_raw.emplace("y", new Value(number_type, out.getOrigin().getY())); + trans_raw.emplace("z", new Value(number_type, out.getOrigin().getZ())); + + CompoundType::Representation rot_raw; + rot_raw.emplace("x", new Value(number_type, out.getRotation().getX())); + rot_raw.emplace("y", new Value(number_type, out.getRotation().getY())); + rot_raw.emplace("z", new Value(number_type, out.getRotation().getZ())); + rot_raw.emplace("w", new Value(number_type, out.getRotation().getW())); + + CompoundType::Representation pose_raw; + pose_raw.emplace("trans", new Value(pose_type.field_type("trans"), std::move(trans_raw))); + pose_raw.emplace("rot", new Value(pose_type.field_type("rot"), std::move(rot_raw))); + pose_raw.emplace("frame", new Value(get_type<StringType>(), to_frame)); + + return Value(pose_type, pose_raw); + + } else if (name == "fawkes_rpy_to_quaternion") { + const CompoundType &quaternion_type = dynamic_cast<const CompoundType &>(ret_type); + tf::Quaternion q = tf::create_quaternion_from_rpy(args.at("roll").numeric_convert<double>(), + args.at("pitch").numeric_convert<double>(), + args.at("yaw").numeric_convert<double>()); + CompoundType::Representation rv_raw; + rv_raw.emplace("x", new Value(number_type, q.getX())); + rv_raw.emplace("y", new Value(number_type, q.getY())); + rv_raw.emplace("z", new Value(number_type, q.getZ())); + rv_raw.emplace("w", new Value(number_type, q.getW())); + + return Value(quaternion_type, rv_raw); + + } else if (name == "fawkes_quaternion_to_rpy") { + const ListType &list_type = dynamic_cast<const ListType &>(ret_type); + + CompoundType::Representation quat_in = + static_cast<CompoundType::Representation>(args.at("quaternion")); + + tf::Quaternion q(quat_in.at("x")->numeric_convert<tf::Scalar>(), + quat_in.at("y")->numeric_convert<tf::Scalar>(), + quat_in.at("z")->numeric_convert<tf::Scalar>(), + quat_in.at("w")->numeric_convert<tf::Scalar>()); + + tf::Scalar roll, pitch, yaw; + q.getEulerZYX(yaw, pitch, roll); + ListType::Representation list_raw; + list_raw.emplace_back(new Value(number_type, roll)); + list_raw.emplace_back(new Value(number_type, pitch)); + list_raw.emplace_back(new Value(number_type, yaw)); + + return Value(list_type, list_raw); + + } else if (name == "fawkes_config_get_float") { + (void)dynamic_cast<const NumberType &>(ret_type); + return Value(number_type, + config_->get_float_or_default(static_cast<std::string>(args.at("path")).c_str(), + args.at("default").numeric_convert<float>())); + } else + throw gologpp::UserError("No such exog_function: " + name); +} + /** Terminate execution of running actions */ void diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.h b/src/plugins/gologpp/gologpp_fawkes_backend.h index 8d0bd956d..79cfea591 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.h +++ b/src/plugins/gologpp/gologpp_fawkes_backend.h @@ -30,8 +30,10 @@ #include <aspect/clock.h> #include <aspect/inifins/inifin.h> #include <blackboard/blackboard.h> +#include <config/config.h> #include <golog++/execution/platform_backend.h> #include <logging/logger.h> +#include <tf/transformer.h> namespace fawkes { class SkillerInterface; @@ -45,14 +47,20 @@ class GologppFawkesBackend : public gologpp::PlatformBackend, public AspectProviderAspect { public: - GologppFawkesBackend(Configuration *config, - std::string cfg_prefix, - Logger * logger, - BlackBoard * blackboard); + GologppFawkesBackend(Configuration * config, + std::string cfg_prefix, + Logger * logger, + BlackBoard * blackboard, + tf::Transformer *tf_listener); virtual ~GologppFawkesBackend(); virtual gologpp::Clock::time_point time() const noexcept override; + virtual gologpp::Value + eval_exog_function(const gologpp::Type & ret_type, + const std::string & name, + const std::unordered_map<std::string, gologpp::Value> &args) override; + private: virtual void terminate_() override; virtual void preempt_activity(std::shared_ptr<gologpp::Activity> a) override; @@ -63,6 +71,8 @@ private: BlackBoard * blackboard_; ActionExecutorDispatcher action_dispatcher_; GologppDispatcherAspectIniFin dispatcher_inifin_; + tf::Transformer * tf_listener_; + Configuration * config_; }; } // namespace gpp - *commit* 563236be9c67a7345a5a0a679dde8ed9ad107e60 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Sat Nov 7 16:40:09 2020 +0100 Subject: gologpp: move skiller interaction into separate class src/plugins/gologpp/action_executor.h | 1 - .../gologpp/aspect/action_executor_dispatcher.cpp | 10 +- src/plugins/gologpp/gologpp_fawkes_backend.cpp | 14 ++- src/plugins/gologpp/gologpp_fawkes_backend.h | 16 +-- src/plugins/gologpp/message_action_executor.cpp | 5 - src/plugins/gologpp/message_action_executor.h | 1 - src/plugins/gologpp/print_action_executor.cpp | 5 - src/plugins/gologpp/print_action_executor.h | 1 - src/plugins/gologpp/remote_skiller_executor.cpp | 13 +-- src/plugins/gologpp/remote_skiller_executor.h | 6 +- src/plugins/gologpp/skiller_action_executor.cpp | 85 +++++----------- src/plugins/gologpp/skiller_action_executor.h | 29 +++--- src/plugins/gologpp/sleep_action_executor.cpp | 5 - src/plugins/gologpp/sleep_action_executor.h | 1 - src/plugins/gologpp/utils.cpp | 110 +++++++++++++++++++++ src/plugins/gologpp/utils.h | 28 ++++++ 16 files changed, 205 insertions(+), 125 deletions(-) _Diff for modified files_: diff --git a/src/plugins/gologpp/action_executor.h b/src/plugins/gologpp/action_executor.h index f9abe9b4d..45a89681f 100644 --- a/src/plugins/gologpp/action_executor.h +++ b/src/plugins/gologpp/action_executor.h @@ -38,7 +38,6 @@ public: virtual void start(std::shared_ptr<gologpp::Activity> activity) = 0; virtual void stop(std::shared_ptr<gologpp::Activity> activity) = 0; virtual bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const = 0; - virtual void terminate() = 0; protected: std::shared_ptr<gologpp::Activity> running_activity_; diff --git a/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp b/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp index c3dca7d59..a6fb48703 100644 --- a/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp +++ b/src/plugins/gologpp/aspect/action_executor_dispatcher.cpp @@ -19,6 +19,7 @@ */ #include "action_executor_dispatcher.h" +#include "../skiller_action_executor.h" #include <core/exception.h> #include <golog++/execution/activity.h> @@ -59,13 +60,16 @@ ActionExecutorDispatcher::register_executor(std::shared_ptr<ActionExecutor> exec action_executors_.push_back(executor); } -/** Terminate all executors +/** Terminate all SkillerManagers */ void ActionExecutorDispatcher::terminate() { - for (auto &executor : action_executors_) - executor->terminate(); + for (auto &executor : action_executors_) { + auto sae = std::dynamic_pointer_cast<SkillerActionExecutor>(executor); + if (sae) + sae->skiller_manager()->terminate(); + } } } // namespace gpp diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.cpp b/src/plugins/gologpp/gologpp_fawkes_backend.cpp index f527f6a31..9109436a9 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.cpp +++ b/src/plugins/gologpp/gologpp_fawkes_backend.cpp @@ -27,6 +27,7 @@ #include "skiller_action_executor.h" #include "sleep_action_executor.h" +#include <blackboard/remote.h> #include <config/config.h> #include <golog++/execution/activity.h> #include <golog++/execution/transition.h> @@ -70,12 +71,15 @@ GologppFawkesBackend::GologppFawkesBackend(Configuration * config, config->get_string_or_default((agent_prefix + "/host").c_str(), "localhost"); const unsigned short int &port = config->get_uint_or_default((agent_prefix + "/port").c_str(), 1910); + std::shared_ptr<SkillerManager> skiller_mgr = + std::make_shared<SkillerManager>(new RemoteBlackBoard(hostname.c_str(), port)); action_dispatcher_.register_executor(std::make_shared<RemoteSkillerActionExecutor>( - logger, "robot", robot, hostname, port, config, cfg_prefix)); + logger, "robot", robot, skiller_mgr, config, cfg_prefix)); } if (config->get_bool_or_default((cfg_prefix + "/use_local_skiller").c_str(), true)) { + local_skiller_mgr_ = std::make_shared<SkillerManager>(blackboard); action_dispatcher_.register_executor( - std::make_shared<SkillerActionExecutor>(logger, blackboard, config, cfg_prefix)); + std::make_shared<SkillerActionExecutor>(logger, local_skiller_mgr_, config, cfg_prefix)); } action_dispatcher_.register_executor( std::make_shared<BBMessageActionExecutor>(logger, blackboard, config, cfg_prefix)); @@ -135,8 +139,8 @@ GologppFawkesBackend::eval_exog_function(const Type & tf::Stamped<tf::Pose> out; tf_listener_->transform_pose(to_frame, - tf::Stamped<tf::Pose>(pose_in, Time(0, 0), from_frame), - out); + tf::Stamped<tf::Pose>(pose_in, Time(0, 0), from_frame), + out); CompoundType::Representation trans_raw; trans_raw.emplace("x", new Value(number_type, out.getOrigin().getX())); @@ -193,7 +197,7 @@ GologppFawkesBackend::eval_exog_function(const Type & (void)dynamic_cast<const NumberType &>(ret_type); return Value(number_type, config_->get_float_or_default(static_cast<std::string>(args.at("path")).c_str(), - args.at("default").numeric_convert<float>())); + args.at("default").numeric_convert<float>())); } else throw gologpp::UserError("No such exog_function: " + name); } diff --git a/src/plugins/gologpp/gologpp_fawkes_backend.h b/src/plugins/gologpp/gologpp_fawkes_backend.h index 79cfea591..47114b762 100644 --- a/src/plugins/gologpp/gologpp_fawkes_backend.h +++ b/src/plugins/gologpp/gologpp_fawkes_backend.h @@ -25,6 +25,7 @@ #include "action_executor.h" #include "aspect/action_executor_dispatcher.h" #include "aspect/action_executor_dispatcher_inifin.h" +#include "utils.h" #include <aspect/aspect_provider.h> #include <aspect/clock.h> @@ -66,13 +67,14 @@ private: virtual void preempt_activity(std::shared_ptr<gologpp::Activity> a) override; virtual void execute_activity(std::shared_ptr<gologpp::Activity>) override; - SkillerInterface * skiller_if_; - Logger * logger_; - BlackBoard * blackboard_; - ActionExecutorDispatcher action_dispatcher_; - GologppDispatcherAspectIniFin dispatcher_inifin_; - tf::Transformer * tf_listener_; - Configuration * config_; + SkillerInterface * skiller_if_; + Logger * logger_; + BlackBoard * blackboard_; + ActionExecutorDispatcher action_dispatcher_; + GologppDispatcherAspectIniFin dispatcher_inifin_; + tf::Transformer * tf_listener_; + Configuration * config_; + std::shared_ptr<SkillerManager> local_skiller_mgr_; }; } // namespace gpp diff --git a/src/plugins/gologpp/message_action_executor.cpp b/src/plugins/gologpp/message_action_executor.cpp index efd507334..525744478 100644 --- a/src/plugins/gologpp/message_action_executor.cpp +++ b/src/plugins/gologpp/message_action_executor.cpp @@ -70,11 +70,6 @@ BBMessageActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> } void -BBMessageActionExecutor::terminate() -{ -} - -void BBMessageActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { diff --git a/src/plugins/gologpp/message_action_executor.h b/src/plugins/gologpp/message_action_executor.h index f71079b11..a6a045401 100644 --- a/src/plugins/gologpp/message_action_executor.h +++ b/src/plugins/gologpp/message_action_executor.h @@ -44,7 +44,6 @@ public: void start(std::shared_ptr<gologpp::Activity> activity) override; void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; - void terminate() override; private: BlackBoard * blackboard_; diff --git a/src/plugins/gologpp/print_action_executor.cpp b/src/plugins/gologpp/print_action_executor.cpp index d2bb4983f..dd6dfc9e9 100644 --- a/src/plugins/gologpp/print_action_executor.cpp +++ b/src/plugins/gologpp/print_action_executor.cpp @@ -53,11 +53,6 @@ PrintActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> act } void -PrintActionExecutor::terminate() -{ -} - -void PrintActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { diff --git a/src/plugins/gologpp/print_action_executor.h b/src/plugins/gologpp/print_action_executor.h index 4c225adec..bf947c235 100644 --- a/src/plugins/gologpp/print_action_executor.h +++ b/src/plugins/gologpp/print_action_executor.h @@ -36,7 +36,6 @@ public: void start(std::shared_ptr<gologpp::Activity> activity) override; void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; - void terminate() override; }; } // namespace gpp } // namespace fawkes diff --git a/src/plugins/gologpp/remote_skiller_executor.cpp b/src/plugins/gologpp/remote_skiller_executor.cpp index 63b2e301b..89ab44a51 100644 --- a/src/plugins/gologpp/remote_skiller_executor.cpp +++ b/src/plugins/gologpp/remote_skiller_executor.cpp @@ -41,27 +41,20 @@ namespace gpp { * @param logger The logger instance to use * @param agent_param_name The parameter key to use for checking if this action should be executed on this agent * @param agent_param_value The name of the remote agent; only execute the action if it matches this agent name - * @param hostname The remote hostname to connect to - * @param port The port to connect to + * @param skiller_mgr The SkillerManager to use (must be connected remotely) * @param config The config to read the skill mapping from * @param cfg_prefix The spec-specific config prefix to use */ RemoteSkillerActionExecutor::RemoteSkillerActionExecutor(Logger * logger, const std::string &agent_param_name, const std::string &agent_param_value, - const std::string &hostname, - unsigned short int port, + std::shared_ptr<SkillerManager> skiller_mgr, Configuration * config, const std::string &cfg_prefix) -: SkillerActionExecutor(logger, new RemoteBlackBoard(hostname.c_str(), port), config, cfg_prefix), +: SkillerActionExecutor(logger, skiller_mgr, config, cfg_prefix), agent_param_name_(agent_param_name), agent_param_value_(agent_param_value) { - blackboard_owner_ = true; -} - -RemoteSkillerActionExecutor::~RemoteSkillerActionExecutor() -{ } bool diff --git a/src/plugins/gologpp/remote_skiller_executor.h b/src/plugins/gologpp/remote_skiller_executor.h index bf6675624..990cd61f2 100644 --- a/src/plugins/gologpp/remote_skiller_executor.h +++ b/src/plugins/gologpp/remote_skiller_executor.h @@ -22,6 +22,8 @@ #include "skiller_action_executor.h" +#include "utils.h" + namespace fawkes { namespace gpp { class RemoteSkillerActionExecutor : public SkillerActionExecutor @@ -30,11 +32,9 @@ public: RemoteSkillerActionExecutor(Logger * logger, const std::string &agent_name_key, const std::string &agent_name_value, - const std::string &hostname, - unsigned short int port, + std::shared_ptr<SkillerManager> skiller_mgr, Configuration * config, const std::string &cfg_prefix); - virtual ~RemoteSkillerActionExecutor() override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; protected: diff --git a/src/plugins/gologpp/skiller_action_executor.cpp b/src/plugins/gologpp/skiller_action_executor.cpp index e31f6575a..6c6258752 100644 --- a/src/plugins/gologpp/skiller_action_executor.cpp +++ b/src/plugins/gologpp/skiller_action_executor.cpp @@ -53,47 +53,25 @@ InvalidArgumentException::InvalidArgumentException(const char *format, ...) : Ex * If the Skiller's status changes, the activity's status is updated accordingly. * @author Till Hofmann * @see ActionSkillMapping - * - * @var SkillerActionExecutor::blackboard_ - * The blackboard to use to access the skiller. - * - * @var SkillerActionExecutor::blackboard_owner_ - * True if this executor is owning its blackboard. */ /** Constructor. * Create and initialize the executor, so a subsequent call to start() directly * starts executing a skill. * @param logger A logger instance to use - * @param blackboard The blackboard to use to connect to the SkillerInterface + * @param skiller_manager The skiller manager * @param config The config to read the skill mapping from * @param cfg_prefix The spec-specific config prefix to use */ -SkillerActionExecutor::SkillerActionExecutor(Logger * logger, - BlackBoard * blackboard, - Configuration * config, - const std::string &cfg_prefix) +SkillerActionExecutor::SkillerActionExecutor(Logger * logger, + std::shared_ptr<SkillerManager> skiller_manager, + Configuration * config, + const std::string & cfg_prefix) : ActionExecutor(logger), - BlackBoardInterfaceListener("Golog++SkillerActionExecutor"), - blackboard_(blackboard), - blackboard_owner_(false), config_(config), + skiller_manager_(skiller_manager), cfg_prefix_(cfg_prefix) { - try { - skiller_if_ = blackboard_->open_for_reading<SkillerInterface>("Skiller"); - } catch (Exception &e) { - logger_->log_error(name(), "Failed to open skiller interface: %s", e.what_no_backtrace()); - } - bbil_add_data_interface(skiller_if_); - blackboard_->register_listener(this, BlackBoard::BBIL_FLAG_DATA); - skiller_if_->read(); - if (!skiller_if_->has_writer()) { - blackboard->unregister_listener(this); - blackboard->close(skiller_if_); - throw Exception("No writer for Skiller interface"); - } - skiller_if_->msgq_enqueue(new SkillerInterface::AcquireControlMessage()); initialize_action_skill_mapping(); } @@ -111,19 +89,6 @@ SkillerActionExecutor::initialize_action_skill_mapping() action_skill_mapping_ = ActionSkillMapping(mapping); } -/** Destructor. - * Close all interfaces and unregister from the blackboard. - */ -SkillerActionExecutor::~SkillerActionExecutor() -{ - skiller_if_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage()); - blackboard_->unregister_listener(this); - blackboard_->close(skiller_if_); - if (blackboard_owner_) { - delete blackboard_; - } -} - /** Check if we can execute the given activity. * Check the action skill mapping whether the given action occurs in the mapping. * If not, we cannot execute the activity. @@ -143,13 +108,14 @@ SkillerActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> a void SkillerActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { + using namespace std::placeholders; if (!can_execute_activity(activity)) { throw Exception("Cannot execute activity '%s' with SkillerActionExecutor", activity->mapped_name().c_str()); } try { - skiller_if_->msgq_enqueue( - new SkillerInterface::ExecSkillMessage(map_activity_to_skill(activity).c_str())); + skiller_manager_->start_skill(map_activity_to_skill(activity), + std::bind(&SkillerActionExecutor::skill_end_handler, this, _1)); running_activity_ = activity; } catch (InvalidArgumentException &e) { logger_->log_error(name(), "Failed to start %s: %s", activity->name().c_str(), e.what()); @@ -165,7 +131,7 @@ void SkillerActionExecutor::stop(std::shared_ptr<gologpp::Activity> activity) { if (*running_activity_ == *activity) { - skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage()); + skiller_manager_->stop_skill(); running_activity_.reset(); } } @@ -212,20 +178,12 @@ SkillerActionExecutor::map_activity_to_skill(std::shared_ptr<gologpp::Activity> } /** Update the status of the activity according to the Skiller status. - * @param iface The interface that has changed + * @param status Skiller status */ void -SkillerActionExecutor::bb_interface_data_changed(Interface *iface) throw() +SkillerActionExecutor::skill_end_handler(SkillerInterface::SkillStatusEnum status) { - if (!running_activity_) { - return; - } - SkillerInterface *skiller_if = dynamic_cast<SkillerInterface *>(iface); - if (!skiller_if) { - return; - } - skiller_if->read(); - switch (skiller_if->status()) { + switch (status) { case SkillerInterface::S_FINAL: running_activity_->update(Transition::Hook::FINISH); running_activity_.reset(); @@ -234,17 +192,20 @@ SkillerActionExecutor::bb_interface_data_changed(Interface *iface) throw() running_activity_->update(Transition::Hook::FAIL); running_activity_.reset(); break; - case SkillerInterface::S_RUNNING: running_activity_->update(Transition::Hook::START); break; - default: break; + default: + logger_->log_error(name(), + "Unexpected skill status %d on %s", + status, + running_activity_->str().c_str()); + break; } } -void -SkillerActionExecutor::terminate() +/** @return the SkillerManager used to execute actions + */ +std::shared_ptr<SkillerManager> SkillerActionExecutor::skiller_manager() { - if (running_activity_) - skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage()); - running_activity_.reset(); + return skiller_manager_; } } // namespace gpp diff --git a/src/plugins/gologpp/skiller_action_executor.h b/src/plugins/gologpp/skiller_action_executor.h index facad1483..b4b71cc46 100644 --- a/src/plugins/gologpp/skiller_action_executor.h +++ b/src/plugins/gologpp/skiller_action_executor.h @@ -22,8 +22,8 @@ #define FAWKES_GOLOGPP_SKILLER_ACTION_EXECUTOR_H #include "action_executor.h" +#include "utils.h" -#include <blackboard/interface_listener.h> #include <utils/misc/map_skill.h> #include <string> @@ -41,32 +41,29 @@ public: InvalidArgumentException(const char *format, ...); }; -class SkillerActionExecutor : public ActionExecutor, public BlackBoardInterfaceListener +class SkillerActionExecutor : public ActionExecutor { public: - SkillerActionExecutor(Logger * logger, - BlackBoard * blackboard, - Configuration * config, - const std::string &cfg_prefix); - virtual ~SkillerActionExecutor() override; - void start(std::shared_ptr<gologpp::Activity> activity) override; - void stop(std::shared_ptr<gologpp::Activity> activity) override; - bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; - virtual void bb_interface_data_changed(Interface *) throw() override; - virtual void terminate() override; + SkillerActionExecutor(Logger * logger, + std::shared_ptr<SkillerManager> skiller_manager, + Configuration * config, + const std::string & cfg_prefix); + void start(std::shared_ptr<gologpp::Activity> activity) override; + void stop(std::shared_ptr<gologpp::Activity> activity) override; + bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; + void skill_end_handler(SkillerInterface::SkillStatusEnum status); + std::shared_ptr<SkillerManager> skiller_manager(); protected: const char *name() const; - BlackBoard *blackboard_; - bool blackboard_owner_; private: void initialize_action_skill_mapping(); std::string map_activity_to_skill(std::shared_ptr<gologpp::Activity> activity); ActionSkillMapping action_skill_mapping_; - SkillerInterface * skiller_if_; Configuration * config_; - const std::string cfg_prefix_; + std::shared_ptr<SkillerManager> skiller_manager_; + const std::string cfg_prefix_; }; } // namespace gpp diff --git a/src/plugins/gologpp/sleep_action_executor.cpp b/src/plugins/gologpp/sleep_action_executor.cpp index 94fe536ba..03f6c42e7 100644 --- a/src/plugins/gologpp/sleep_action_executor.cpp +++ b/src/plugins/gologpp/sleep_action_executor.cpp @@ -62,11 +62,6 @@ SleepActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> act } void -SleepActionExecutor::terminate() -{ -} - -void SleepActionExecutor::start(std::shared_ptr<gologpp::Activity> activity) { if (!can_execute_activity(activity)) { diff --git a/src/plugins/gologpp/sleep_action_executor.h b/src/plugins/gologpp/sleep_action_executor.h index 8dff21d9d..0b54bc0d9 100644 --- a/src/plugins/gologpp/sleep_action_executor.h +++ b/src/plugins/gologpp/sleep_action_executor.h @@ -39,7 +39,6 @@ public: void start(std::shared_ptr<gologpp::Activity> activity) override; void stop(std::shared_ptr<gologpp::Activity> activity) override; bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const override; - void terminate() override; private: std::list<std::future<void>> running_sleeps_; diff --git a/src/plugins/gologpp/utils.cpp b/src/plugins/gologpp/utils.cpp index 89b498d94..6706b1e14 100644 --- a/src/plugins/gologpp/utils.cpp +++ b/src/plugins/gologpp/utils.cpp @@ -21,6 +21,7 @@ #include "utils.h" #include <blackboard/blackboard.h> +#include <blackboard/remote.h> #include <golog++/model/value.h> using gologpp::Value; @@ -215,5 +216,114 @@ field_to_value(InterfaceFieldIterator &fi, unsigned int idx) throw Exception("Unhandled interface field type"); } +/** A central point to interact with the skiller. There can only be one of these since it + * acquires exclusive control + * @param logger Initialized logger to use + * @param blackboard Initialized Blackboard to use. Deleted in destructor if it's a @ref RemoteBlackBoard. + */ +SkillerManager::SkillerManager(BlackBoard *blackboard) +: BlackBoardInterfaceListener("Golog++SkillerManager"), blackboard_(blackboard), terminated_(false) +{ + skiller_iface_ = blackboard_->open_for_reading<SkillerInterface>("Skiller"); + skiller_iface_->read(); + if (!skiller_iface_->has_writer()) { + blackboard_->close(skiller_iface_); + delete blackboard_; + throw Exception("No writer for Skiller interface"); + } + + bbil_add_data_interface(skiller_iface_); + blackboard_->register_listener(this, BlackBoard::BBIL_FLAG_DATA); + + skiller_iface_->msgq_enqueue(new SkillerInterface::AcquireControlMessage()); +} + +/** Release exclusive control and free all resources + */ +SkillerManager::~SkillerManager() +{ + skiller_iface_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage()); + blackboard_->unregister_listener(this); + blackboard_->close(skiller_iface_); + if (dynamic_cast<RemoteBlackBoard *>(blackboard_)) + delete blackboard_; +} + +/** Start the given skillstring and call @ref end_cb when it ends + * @param skillstring Skillstring to run + * @param end_cb End callback. Status enum is either S_FINAL or S_FAILED. + */ +void +SkillerManager::start_skill(const std::string &skillstring, + const std::function<void(SkillerInterface::SkillStatusEnum)> &end_cb) +{ + std::lock_guard<std::recursive_mutex> l(mutex_); + + if (terminated_) + return; + + skiller_iface_->read(); + if (end_callback_) { + throw IllegalArgumentException("Cannot execute \"%s\" while \"%s\" is still running", + skillstring.c_str(), + skiller_iface_->skill_string()); + } + + skiller_iface_->msgq_enqueue(new SkillerInterface::ExecSkillMessage(skillstring.c_str())); + end_callback_ = end_cb; +} + +/** Stop execution of current skill (if any) + */ +void SkillerManager::stop_skill() +{ + std::lock_guard<std::recursive_mutex> l(mutex_); + + if (terminated_) + return; + + if (end_callback_) + skiller_iface_->msgq_enqueue(new SkillerInterface::StopExecMessage()); + + end_callback_ = nullptr; +} + +void +SkillerManager::bb_interface_data_refreshed(Interface *iface) throw() +{ + std::lock_guard<std::recursive_mutex> l(mutex_); + + if (terminated_ || !end_callback_) + return; + + SkillerInterface *skiller_if = dynamic_cast<SkillerInterface *>(iface); + if (!skiller_if) { + return; + } + + skiller_if->read(); + if (skiller_if->status() == SkillerInterface::SkillStatusEnum::S_FINAL + || skiller_if->status() == SkillerInterface::SkillStatusEnum::S_FAILED) { + end_callback_(skiller_if->status()); + end_callback_ = nullptr; + } +} + +/** Prepare for shutdown by stopping skill execution. + */ +void SkillerManager::terminate() +{ + std::lock_guard<std::recursive_mutex> l(mutex_); + + stop_skill(); + terminated_ = true; +} + +constexpr const char * +SkillerManager::name() const +{ + return "Golog++SkillerManager"; +} + } // namespace gpp } // namespace fawkes diff --git a/src/plugins/gologpp/utils.h b/src/plugins/gologpp/utils.h index 346c2bf7a..f2983edf0 100644 --- a/src/plugins/gologpp/utils.h +++ b/src/plugins/gologpp/utils.h @@ -20,9 +20,14 @@ #pragma once +#include <blackboard/interface_listener.h> #include <golog++/model/value.h> +#include <interfaces/SkillerInterface.h> +#include <logging/logger.h> #include <boost/variant/variant.hpp> +#include <functional> +#include <mutex> namespace gologpp { class Value; @@ -56,5 +61,28 @@ private: void value_to_field(const gologpp::Value &value, InterfaceFieldIterator *field); gologpp::Value *field_to_value(InterfaceFieldIterator &fi, unsigned int idx); +class SkillerManager : public BlackBoardInterfaceListener +{ +public: + SkillerManager(BlackBoard *blackboard); + ~SkillerManager(); + + void start_skill(const std::string & skillstring, + const std::function<void(SkillerInterface::SkillStatusEnum)> &end_cb); + void stop_skill(); + + virtual void bb_interface_data_refreshed(Interface *) throw() override; + void terminate(); + +private: + constexpr const char *name() const; + + BlackBoard * blackboard_; + std::function<void(SkillerInterface::SkillStatusEnum)> end_callback_; + SkillerInterface *skiller_iface_; + std::recursive_mutex mutex_; + bool terminated_; +}; + } // namespace gpp } // namespace fawkes - *commit* 817b6417a7c968349056c87ff2f366dbc59c6490 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Tue Nov 10 03:06:42 2020 +0100 Subject: gologpp: fix non-virtual d'tor warning src/plugins/gologpp/action_executor.h | 1 + 1 file changed, 1 insertion(+) _Diff for modified files_: diff --git a/src/plugins/gologpp/action_executor.h b/src/plugins/gologpp/action_executor.h index 45a89681f..8fda989d4 100644 --- a/src/plugins/gologpp/action_executor.h +++ b/src/plugins/gologpp/action_executor.h @@ -35,6 +35,7 @@ class ActionExecutor { public: ActionExecutor(Logger *logger); + virtual ~ActionExecutor() = default; virtual void start(std::shared_ptr<gologpp::Activity> activity) = 0; virtual void stop(std::shared_ptr<gologpp::Activity> activity) = 0; virtual bool can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const = 0; - *commit* f12f024147c528425c3a5b93538da484f9cd284a - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Sat Nov 14 16:18:38 2020 +0100 Subject: gologpp: prevent premature exog event crash src/plugins/gologpp/execution_thread.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) _Diff for modified files_: diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index 9f2ca7acf..24fb07555 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -93,8 +93,6 @@ GologppThread::init() logger->log_info(name(), "Initializing ReadyLog context..."); - exog_mgr_ = new ExogManager(this, config, spec_cfg_prefix, blackboard, logger); - gologpp::eclipse_opts options; options.trace = config->get_bool_or_default((cfg_prefix + "/trace").c_str(), false); options.guitrace = options.trace; @@ -102,6 +100,8 @@ GologppThread::init() std::make_unique<GologppFawkesBackend>( config, spec_cfg_prefix, logger, blackboard, tf_listener)); + exog_mgr_ = new ExogManager(this, config, spec_cfg_prefix, blackboard, logger); + logger->log_info(name(), "... initialization done"); } - *commit* 2175ef6c5c115a2e2c3a1a3b5b168912cab25240 - - - - - - - - - - Author: Victor Mataré <[email protected]> Date: Sat Nov 14 16:19:56 2020 +0100 Subject: gologpp: catch gologpp::EngineError src/plugins/gologpp/execution_thread.cpp | 2 ++ 1 file changed, 2 insertions(+) _Diff for modified files_: diff --git a/src/plugins/gologpp/execution_thread.cpp b/src/plugins/gologpp/execution_thread.cpp index 24fb07555..5d311c14a 100644 --- a/src/plugins/gologpp/execution_thread.cpp +++ b/src/plugins/gologpp/execution_thread.cpp @@ -116,6 +116,8 @@ GologppThread::once() logger->log_error(name(), "User Error: %s", e.what()); } catch (gologpp::EclipseError &e) { logger->log_error(name(), "Eclipse Error: %s", e.what()); + } catch (gologpp::EngineError &e) { + logger->log_error(name(), "Engine error: %s", e.what()); } }
_______________________________________________ fawkes-commits mailing list [email protected] https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
