Changes have been pushed for the repository "fawkesrobotics/fawkes".
Clone: https://github.com/fawkesrobotics/fawkes.git Gitweb: https://github.com/fawkesrobotics/fawkes The branch, thofmann/skiller-execution-time-estimators has been updated to 2b32d12e18ae42c91a94b6190731debe06168c9e (commit) via ddc43282c88d3ef089be0ea38548e80fe7bceaa0 (commit) via 8162992439b35063e423bde912ce38f10d489839 (commit) via bd9886a3c0b4e24639e09ee45823d85623a126b0 (commit) via 389bf9dac739e75e1cacdaf868ea951588c9194a (commit) via af6039ccfb6427094770e32d9c5d8982d597077d (commit) via 41ab14bf9d4ec19c45d40effce9d64ed02191d52 (commit) via 5d948432e21feb494bc450030757917556e4ad1f (commit) via 2fa8f892f60c063926bc754ea3046a9d8dcba768 (commit) via 8dd6a3111ccd310c79ff0b5c7db9e43be5b24ff0 (commit) via 94d4e001ab802dd77f5c124d85c547f2da3cb27d (commit) via ae371605e8052373cf1bc6b23e5ee4bff11c6a59 (commit) from 75dff5e493640e5b0447fdd67741c2d686b6a26d (commit) https://github.com/fawkesrobotics/fawkes/tree/thofmann/skiller-execution-time-estimators 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 ae371605e8052373cf1bc6b23e5ee4bff11c6a59 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Jan 7 17:17:51 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Tue Jan 7 17:17:51 2020 +0100 skiller-simulator-navgraph: add skill time estimator for moving skills The plugin adds an estimator for the skill `goto` to the skiller-simulator. It estimates the skill execution time using the distance between the current position and the navgraph node that we are moving to. https://github.com/fawkesrobotics/fawkes/commit/ae371605e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 94d4e001ab802dd77f5c124d85c547f2da3cb27d Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Jan 7 18:09:17 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Tue Jan 7 18:09:17 2020 +0100 skiller-simulator: print info about exec time https://github.com/fawkesrobotics/fawkes/commit/94d4e001a - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 8dd6a3111ccd310c79ff0b5c7db9e43be5b24ff0 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Jan 7 18:10:33 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Tue Jan 7 18:10:33 2020 +0100 skiller-simulator: also allow skills with newlines We actually allow skill strings to contain newlines. Simply remove all newline characters before parsing the skill. https://github.com/fawkesrobotics/fawkes/commit/8dd6a3111 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 2fa8f892f60c063926bc754ea3046a9d8dcba768 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:24:03 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:24:03 2020 +0100 skiller-simulator: let the estimator know that we executed the skill The estimated time may depend on what we did previously. In order to let the estimator know what was done before, always let it know that we actually executed a skill. https://github.com/fawkesrobotics/fawkes/commit/2fa8f892f - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 5d948432e21feb494bc450030757917556e4ad1f Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:39:24 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:39:24 2020 +0100 fixup! skiller-simulator-navgraph: add skill time estimator for moving skills https://github.com/fawkesrobotics/fawkes/commit/5d948432e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 41ab14bf9d4ec19c45d40effce9d64ed02191d52 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:39:37 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:39:37 2020 +0100 skiller-simulator-navgraph: remember the last position Reading the current position from the Pose interface does not work because the interface is not updated without a running AMCL. Instead, remember the last position that we moved to and use that to compute the distance. https://github.com/fawkesrobotics/fawkes/commit/41ab14bf9 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit af6039ccfb6427094770e32d9c5d8982d597077d Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:43:33 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:45:29 2020 +0100 skiller-simulator-navgraph: also use speed to compute the estimated time The simulated speed of the robot is a configurable and defaults to 0.5. https://github.com/fawkesrobotics/fawkes/commit/af6039ccf - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 389bf9dac739e75e1cacdaf868ea951588c9194a Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:58:55 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:58:55 2020 +0100 fixup! skiller-simulator-navgraph: remember the last position https://github.com/fawkesrobotics/fawkes/commit/389bf9dac - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit bd9886a3c0b4e24639e09ee45823d85623a126b0 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 12:59:36 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 12:59:36 2020 +0100 skiller-simulator-navgraph: make the set of skills configurable We may have more skills than goto that essentially estimate the travel time. Make this list configurable. https://github.com/fawkesrobotics/fawkes/commit/bd9886a3c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 8162992439b35063e423bde912ce38f10d489839 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 13:04:27 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 13:04:27 2020 +0100 skiller-simulator: use config prefix 'skiller-simulator' The config prefix should actually match the plugin name. https://github.com/fawkesrobotics/fawkes/commit/816299243 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit ddc43282c88d3ef089be0ea38548e80fe7bceaa0 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 13:05:52 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 13:05:52 2020 +0100 skiller-simulator-navgraph: use config prefix 'skiller-simulator' The config prefix should actually match the plugin name. https://github.com/fawkesrobotics/fawkes/commit/ddc43282c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 2b32d12e18ae42c91a94b6190731debe06168c9e Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Thu Jan 9 13:10:16 2020 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Thu Jan 9 13:10:16 2020 +0100 config: add default config for the skiller-simulator https://github.com/fawkesrobotics/fawkes/commit/2b32d12e1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- cfg/conf.d/skiller-simulator.yaml | 12 ++++ src/plugins/Makefile | 3 +- .../skiller-simulator-navgraph}/Makefile | 31 ++++------ .../navgraph_estimator.cpp | 68 ++++++++++++++++++++++ .../navgraph_estimator.h} | 26 ++++++--- .../skiller_simulator_navgraph_plugin.cpp} | 29 +++++---- .../skiller_simulator_navgraph_thread.cpp | 43 ++++++++++++++ .../skiller_simulator_navgraph_thread.h} | 40 +++++-------- .../estimators/config_estimator.h | 2 +- src/plugins/skiller-simulator/exec_thread.cpp | 16 ++++- src/plugins/skiller-simulator/exec_thread.h | 1 + .../skiller-simulator/execution_time_estimator.cpp | 14 ++++- .../skiller-simulator/execution_time_estimator.h | 1 + .../skiller-simulator/tests/test_skill_parser.cpp | 8 +++ 14 files changed, 220 insertions(+), 74 deletions(-) create mode 100644 cfg/conf.d/skiller-simulator.yaml copy src/{tools/pddl_parser => plugins/skiller-simulator-navgraph}/Makefile (50%) create mode 100644 src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp copy src/plugins/{skiller-simulator/estimators/config_estimator.h => skiller-simulator-navgraph/navgraph_estimator.h} (59%) copy src/plugins/{attic/webview-ptzcam/webview-ptzcam-plugin.cpp => skiller-simulator-navgraph/skiller_simulator_navgraph_plugin.cpp} (53%) create mode 100644 src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp copy src/plugins/{plexil/navgraph_access_thread.h => skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h} (55%) - *Diffs* ------------------------------------------------------------- - *commit* ae371605e8052373cf1bc6b23e5ee4bff11c6a59 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Jan 7 17:17:51 2020 +0100 Subject: skiller-simulator-navgraph: add skill time estimator for moving skills src/plugins/Makefile | 3 +- src/plugins/skiller-simulator-navgraph/Makefile | 33 +++++++++++++++ .../navgraph_estimator.cpp} | 36 ++++++++-------- .../navgraph_estimator.h} | 20 ++++----- .../skiller_simulator_navgraph_plugin.cpp} | 28 ++++--------- .../skiller_simulator_navgraph_thread.cpp | 48 ++++++++++++++++++++++ .../skiller_simulator_navgraph_thread.h} | 41 ++++++++---------- 7 files changed, 138 insertions(+), 71 deletions(-) _Diff for modified files_: diff --git a/src/plugins/Makefile b/src/plugins/Makefile index 64726eb18..424e47ba0 100644 --- a/src/plugins/Makefile +++ b/src/plugins/Makefile @@ -21,7 +21,7 @@ SUBDIRS = bbsync bblogger webview ttmainloop rrd \ laser imu flite festival joystick openrave \ katana jaco pantilt roomba nao robotino \ bumblebee2 realsense realsense2 perception amcl \ - skiller skiller-simulator luaagent \ + skiller skiller-simulator skiller-simulator-navgraph luaagent \ laser-filter laser-lines laser-cluster laser-pointclouds \ static_transforms navgraph navgraph-clusters navgraph-generator colli \ clips clips-agent clips-protobuf clips-navgraph \ @@ -52,6 +52,7 @@ pddl-planner: robot-memory gazebo: robotino skiller: navgraph skiller-simulator: skiller +skiller-simulator-navgraph: skiller-simulator perception: mongodb navgraph-generator: navgraph amcl openprs-agent: openprs - *commit* 94d4e001ab802dd77f5c124d85c547f2da3cb27d - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Jan 7 18:09:17 2020 +0100 Subject: skiller-simulator: print info about exec time src/plugins/skiller-simulator/exec_thread.cpp | 4 ++++ 1 file changed, 4 insertions(+) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator/exec_thread.cpp b/src/plugins/skiller-simulator/exec_thread.cpp index aa130585c..ae1f5cb65 100644 --- a/src/plugins/skiller-simulator/exec_thread.cpp +++ b/src/plugins/skiller-simulator/exec_thread.cpp @@ -122,6 +122,10 @@ SkillerSimulatorExecutionThread::loop() skiller_if_->set_msgid(m->id()); skiller_if_->set_error(""); skiller_if_->set_status(SkillerInterface::S_RUNNING); + logger->log_info(name(), + "Executing '%s', will take %.2f seconds", + m->skill_string(), + get_skill_runtime(m->skill_string())); skill_starttime_ = Time(); write_interface = true; skill_enqueued = true; - *commit* 8dd6a3111ccd310c79ff0b5c7db9e43be5b24ff0 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Jan 7 18:10:33 2020 +0100 Subject: skiller-simulator: also allow skills with newlines src/plugins/skiller-simulator/execution_time_estimator.cpp | 9 ++++++--- src/plugins/skiller-simulator/tests/test_skill_parser.cpp | 8 ++++++++ 2 files changed, 14 insertions(+), 3 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator/execution_time_estimator.cpp b/src/plugins/skiller-simulator/execution_time_estimator.cpp index 260e96707..20518cc6c 100644 --- a/src/plugins/skiller-simulator/execution_time_estimator.cpp +++ b/src/plugins/skiller-simulator/execution_time_estimator.cpp @@ -54,12 +54,15 @@ using Skill = ExecutionTimeEstimator::Skill; */ Skill::Skill(const std::string &skill_string) { - if (skill_string.empty()) { + std::string skill_no_newlines(skill_string); + skill_no_newlines.erase(std::remove(skill_no_newlines.begin(), skill_no_newlines.end(), '\n'), + skill_no_newlines.end()); + if (skill_no_newlines.empty()) { return; } const std::regex regex("(\\w+)(?:\\(\\)|\\{(.+)?})"); std::smatch match; - if (std::regex_match(skill_string, match, regex)) { + if (std::regex_match(skill_no_newlines, match, regex)) { assert(match.size() > 1); skill_name = match[1]; if (match.size() > 2) { @@ -67,7 +70,7 @@ Skill::Skill(const std::string &skill_string) parse_args(args); } } else { - throw Exception("Unexpected skill string"); + throw Exception("Unexpected skill string: '%s'", skill_no_newlines.c_str()); } } diff --git a/src/plugins/skiller-simulator/tests/test_skill_parser.cpp b/src/plugins/skiller-simulator/tests/test_skill_parser.cpp index f25c0ab2c..ead2e478f 100644 --- a/src/plugins/skiller-simulator/tests/test_skill_parser.cpp +++ b/src/plugins/skiller-simulator/tests/test_skill_parser.cpp @@ -66,3 +66,11 @@ TEST(SkillParserTest, SkillWithMultipleArgs) ASSERT_EQ(s.skill_args["text"], "hello"); ASSERT_EQ(s.skill_args["second"], "bye"); } + +TEST(SkillParserTest, SkillWithNewline) +{ + Skill s("say{text=\n\"hello\"}"); + ASSERT_EQ(s.skill_name, "say"); + ASSERT_EQ(s.skill_args.size(), 1); + ASSERT_EQ(s.skill_args["text"], "hello"); +} - *commit* 2fa8f892f60c063926bc754ea3046a9d8dcba768 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:24:03 2020 +0100 Subject: skiller-simulator: let the estimator know that we executed the skill src/plugins/skiller-simulator/exec_thread.cpp | 10 ++++++++++ src/plugins/skiller-simulator/exec_thread.h | 1 + src/plugins/skiller-simulator/execution_time_estimator.cpp | 5 +++++ src/plugins/skiller-simulator/execution_time_estimator.h | 1 + 4 files changed, 17 insertions(+) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator/exec_thread.cpp b/src/plugins/skiller-simulator/exec_thread.cpp index ae1f5cb65..d10f350cd 100644 --- a/src/plugins/skiller-simulator/exec_thread.cpp +++ b/src/plugins/skiller-simulator/exec_thread.cpp @@ -158,6 +158,7 @@ SkillerSimulatorExecutionThread::loop() Time now = Time(); if (Time() > skill_starttime_ + get_skill_runtime(skiller_if_->skill_string())) { logger->log_info(name(), "Skill '%s' is final", skiller_if_->skill_string()); + execute_skill(skiller_if_->skill_string()); skiller_if_->set_skill_string(""); skiller_if_->set_status(SkillerInterface::S_FINAL); write_interface = true; @@ -186,3 +187,12 @@ SkillerSimulatorExecutionThread::get_skill_runtime(const std::string &skill) con return default_skill_runtime_; } } + +void +SkillerSimulatorExecutionThread::execute_skill(const std::string &skill) +{ + auto provider = execution_time_estimator_manager_.get_provider(skill); + if (provider) { + (*provider)->execute(skill); + } +} diff --git a/src/plugins/skiller-simulator/exec_thread.h b/src/plugins/skiller-simulator/exec_thread.h index 73e9c67e2..b0439aa1d 100644 --- a/src/plugins/skiller-simulator/exec_thread.h +++ b/src/plugins/skiller-simulator/exec_thread.h @@ -57,6 +57,7 @@ protected: private: float get_skill_runtime(const std::string &skill) const; + void execute_skill(const std::string &skill); fawkes::SkillerInterface *skiller_if_; float default_skill_runtime_; fawkes::Time skill_starttime_; diff --git a/src/plugins/skiller-simulator/execution_time_estimator.cpp b/src/plugins/skiller-simulator/execution_time_estimator.cpp index 20518cc6c..9388befff 100644 --- a/src/plugins/skiller-simulator/execution_time_estimator.cpp +++ b/src/plugins/skiller-simulator/execution_time_estimator.cpp @@ -46,6 +46,11 @@ using Skill = ExecutionTimeEstimator::Skill; * Check if this estimator can give an estimate for the given * @param skill The skill object to check. * @return true if this estimator can give an execution time estimate for the given skill. + * + * @fn void ExecutionTimeEstimator::execute(const Skill &skill) const + * Let the estimator know that we are executing this skill, so it can apply + * possible side effects. + * @param skill The skill to execute */ /** Constructor. diff --git a/src/plugins/skiller-simulator/execution_time_estimator.h b/src/plugins/skiller-simulator/execution_time_estimator.h index 16236df6c..7cc58fcae 100644 --- a/src/plugins/skiller-simulator/execution_time_estimator.h +++ b/src/plugins/skiller-simulator/execution_time_estimator.h @@ -47,6 +47,7 @@ public: virtual float get_execution_time(const Skill &skill) const = 0; virtual bool can_execute(const Skill &skill) const = 0; + virtual void execute(const Skill &skill){}; }; } // namespace skiller_simulator - *commit* 5d948432e21feb494bc450030757917556e4ad1f - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:39:24 2020 +0100 Subject: fixup! skiller-simulator-navgraph: add skill time estimator for moving skills .../skiller-simulator-navgraph/navgraph_estimator.cpp | 12 ++++++++++-- .../skiller_simulator_navgraph_plugin.cpp | 7 +++++++ .../skiller_simulator_navgraph_thread.cpp | 3 ++- 3 files changed, 19 insertions(+), 3 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp index 4988ed45f..8278afe78 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp @@ -20,10 +20,18 @@ #include "navgraph_estimator.h" -#include "interfaces/Position3DInterface.h" - namespace fawkes { namespace skiller_simulator { + +/** @class NavGraphEstimator + * Estimate the execution time for the skill goto by querying the distance from + * the navgraph. + */ + +/** Constructor. + * @param navgraph The navgraph to read the node positions from + * @param pose The Position3DInterface to read the current position from + */ NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Position3DInterface *pose) : navgraph_(navgraph), pose_(pose) { diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_plugin.cpp b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_plugin.cpp index 695d786cc..9b2e36230 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_plugin.cpp +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_plugin.cpp @@ -22,9 +22,16 @@ #include <core/plugin.h> +/** @class SkillerSimulatorNavgraphPlugin + * Plugin to get estimates for skill execution times from the navgraph. + */ + class SkillerSimulatorNavgraphPlugin : public fawkes::Plugin { public: + /** Constructor. + * @param config The fawkes config to use + */ explicit SkillerSimulatorNavgraphPlugin(fawkes::Configuration *config) : Plugin(config) { thread_list.push_back(new SkillerSimulatorNavgraphEstimatorThread()); diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp index b5162d49c..15dac3834 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp @@ -24,10 +24,11 @@ #include <interfaces/Position3DInterface.h> -/** @class SkillerSimulatorNavgraphEstimatorAspect +/** @class SkillerSimulatorNavgraphEstimatorThread * Get estimates for skill execution times from the navgraph. */ +/** Constructor. */ SkillerSimulatorNavgraphEstimatorThread::SkillerSimulatorNavgraphEstimatorThread() : Thread("SkillerSimulatorNavgraphEstimatorThread", Thread::OPMODE_WAITFORWAKEUP) { - *commit* 41ab14bf9d4ec19c45d40effce9d64ed02191d52 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:39:37 2020 +0100 Subject: skiller-simulator-navgraph: remember the last position src/plugins/skiller-simulator-navgraph/Makefile | 1 - .../navgraph_estimator.cpp | 21 +++++++++++++++------ .../skiller-simulator-navgraph/navgraph_estimator.h | 9 ++++++--- .../skiller_simulator_navgraph_thread.cpp | 14 +------------- .../skiller_simulator_navgraph_thread.h | 11 +++-------- 5 files changed, 25 insertions(+), 31 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/Makefile b/src/plugins/skiller-simulator-navgraph/Makefile index 1c193237a..7331e51c0 100644 --- a/src/plugins/skiller-simulator-navgraph/Makefile +++ b/src/plugins/skiller-simulator-navgraph/Makefile @@ -19,7 +19,6 @@ include $(BASEDIR)/etc/buildsys/config.mk LIBS_skiller_simulator_navgraph = fawkescore fawkesutils fawkesaspects \ fawkesblackboard fawkesinterface \ - Position3DInterface \ fawkeslogging fawkes_skiller_time_estimator OBJS_skiller_simulator_navgraph = skiller_simulator_navgraph_plugin.o \ skiller_simulator_navgraph_thread.o \ diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp index 8278afe78..999d9be0c 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp @@ -30,11 +30,13 @@ namespace skiller_simulator { /** Constructor. * @param navgraph The navgraph to read the node positions from - * @param pose The Position3DInterface to read the current position from + * @param config The config to read the initial position from */ -NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Position3DInterface *pose) -: navgraph_(navgraph), pose_(pose) +NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Configuration *config) +: navgraph_(navgraph) { + last_pose_x_ = config->get_float_or_default("plugins/amcl/init_pose_x", 0); + last_pose_y_ = config->get_float_or_default("plugins/amcl/init_pose_y", 0); } bool @@ -46,9 +48,16 @@ NavGraphEstimator::can_execute(const Skill &skill) const float NavGraphEstimator::get_execution_time(const Skill &skill) const { - pose_->read(); - return navgraph_->node(skill.skill_args.at("place")) - .distance(pose_->translation(0), pose_->translation(1)); + return navgraph_->node(skill.skill_args.at("place")).distance(last_pose_x_, last_pose_y_); } + +void +NavGraphEstimator::execute(const Skill &skill) +{ + auto node = navgraph_->node(skill.skill_args.at("place")); + last_pose_x_ = node.x(); + last_pose_y_ = node.y(); +} + } // namespace skiller_simulator } // namespace fawkes diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h index 0162d5ef6..22c6964cc 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h @@ -22,6 +22,7 @@ #include "interfaces/Position3DInterface.h" +#include <config/config.h> #include <navgraph/navgraph.h> #include <plugins/skiller-simulator/execution_time_estimator.h> @@ -30,13 +31,15 @@ namespace skiller_simulator { class NavGraphEstimator : public ExecutionTimeEstimator { public: - NavGraphEstimator(LockPtr<NavGraph> navgraph, Position3DInterface *pose); + NavGraphEstimator(LockPtr<NavGraph> navgraph, Configuration *config); float get_execution_time(const Skill &skill) const override; bool can_execute(const Skill &skill) const override; + void execute(const Skill &skill) override; private: - LockPtr<NavGraph> navgraph_; - Position3DInterface *pose_; + LockPtr<NavGraph> navgraph_; + float last_pose_x_; + float last_pose_y_; }; } // namespace skiller_simulator } // namespace fawkes diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp index 15dac3834..9c866887f 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp @@ -32,18 +32,6 @@ SkillerSimulatorNavgraphEstimatorThread::SkillerSimulatorNavgraphEstimatorThread() : Thread("SkillerSimulatorNavgraphEstimatorThread", Thread::OPMODE_WAITFORWAKEUP) { -} - -void -SkillerSimulatorNavgraphEstimatorThread::init() -{ - pose_ = blackboard->open_for_reading<fawkes::Position3DInterface>("Pose"); execution_time_estimator_manager_->register_provider( - std::make_shared<fawkes::skiller_simulator::NavGraphEstimator>(navgraph, pose_)); -} - -void -SkillerSimulatorNavgraphEstimatorThread::finalize() -{ - blackboard->close(pose_); + std::make_shared<fawkes::skiller_simulator::NavGraphEstimator>(navgraph, config)); } diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h index 087b2a1b9..cf30919fa 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h @@ -20,24 +20,19 @@ #pragma once -#include "interfaces/Position3DInterface.h" -#include "navgraph/aspect/navgraph.h" - #include <aspect/blackboard.h> +#include <aspect/configurable.h> #include <core/threading/thread.h> +#include <navgraph/aspect/navgraph.h> #include <plugins/skiller-simulator/execution_time_estimator_aspect/execution_time_estimator_aspect.h> class SkillerSimulatorNavgraphEstimatorThread : public fawkes::Thread, public fawkes::BlackBoardAspect, + public fawkes::ConfigurableAspect, public fawkes::NavGraphAspect, public fawkes::skiller_simulator::ExecutionTimeEstimatorsAspect { public: SkillerSimulatorNavgraphEstimatorThread(); - virtual void init(); - virtual void finalize(); - -private: - fawkes::Position3DInterface *pose_; }; - *commit* af6039ccfb6427094770e32d9c5d8982d597077d - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:43:33 2020 +0100 Subject: skiller-simulator-navgraph: also use speed to compute the estimated time src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp | 5 ++++- src/plugins/skiller-simulator-navgraph/navgraph_estimator.h | 1 + 2 files changed, 5 insertions(+), 1 deletion(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp index 999d9be0c..52436f7e3 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp @@ -37,6 +37,8 @@ NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Configuration * { last_pose_x_ = config->get_float_or_default("plugins/amcl/init_pose_x", 0); last_pose_y_ = config->get_float_or_default("plugins/amcl/init_pose_y", 0); + speed_ = + config->get_float_or_default("plugins/skiller-simulation/estimators/navgraph/speed", 0.5); } bool @@ -48,7 +50,8 @@ NavGraphEstimator::can_execute(const Skill &skill) const float NavGraphEstimator::get_execution_time(const Skill &skill) const { - return navgraph_->node(skill.skill_args.at("place")).distance(last_pose_x_, last_pose_y_); + return navgraph_->node(skill.skill_args.at("place")).distance(last_pose_x_, last_pose_y_) + / speed_; } void diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h index 22c6964cc..6da629fa0 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h @@ -40,6 +40,7 @@ private: LockPtr<NavGraph> navgraph_; float last_pose_x_; float last_pose_y_; + float speed_; }; } // namespace skiller_simulator } // namespace fawkes - *commit* 389bf9dac739e75e1cacdaf868ea951588c9194a - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:58:55 2020 +0100 Subject: fixup! skiller-simulator-navgraph: remember the last position .../skiller_simulator_navgraph_thread.cpp | 6 ++++++ .../skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h | 1 + 2 files changed, 7 insertions(+) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp index 9c866887f..4693e5e2a 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.cpp @@ -32,6 +32,12 @@ SkillerSimulatorNavgraphEstimatorThread::SkillerSimulatorNavgraphEstimatorThread() : Thread("SkillerSimulatorNavgraphEstimatorThread", Thread::OPMODE_WAITFORWAKEUP) { +} + +/** Initializer. */ +void +SkillerSimulatorNavgraphEstimatorThread::init() +{ execution_time_estimator_manager_->register_provider( std::make_shared<fawkes::skiller_simulator::NavGraphEstimator>(navgraph, config)); } diff --git a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h index cf30919fa..5f872176c 100644 --- a/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h +++ b/src/plugins/skiller-simulator-navgraph/skiller_simulator_navgraph_thread.h @@ -35,4 +35,5 @@ class SkillerSimulatorNavgraphEstimatorThread { public: SkillerSimulatorNavgraphEstimatorThread(); + void init(); }; - *commit* bd9886a3c0b4e24639e09ee45823d85623a126b0 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 12:59:36 2020 +0100 Subject: skiller-simulator-navgraph: make the set of skills configurable .../skiller-simulator-navgraph/navgraph_estimator.cpp | 5 ++++- src/plugins/skiller-simulator-navgraph/navgraph_estimator.h | 12 ++++++++---- 2 files changed, 12 insertions(+), 5 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp index 52436f7e3..5d9bcadfd 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp @@ -39,12 +39,15 @@ NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Configuration * last_pose_y_ = config->get_float_or_default("plugins/amcl/init_pose_y", 0); speed_ = config->get_float_or_default("plugins/skiller-simulation/estimators/navgraph/speed", 0.5); + skills_ = config->get_strings_or_defaults("plugins/skiller-simulation/estimators/navgraph/skills", + {"goto"}); } bool NavGraphEstimator::can_execute(const Skill &skill) const { - return skill.skill_name == "goto" && navgraph_->node_exists(skill.skill_args.at("place")); + return std::find(skills_.begin(), skills_.end(), skill.skill_name) != skills_.end() + && navgraph_->node_exists(skill.skill_args.at("place")); } float diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h index 6da629fa0..6b2e39e3d 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.h @@ -26,6 +26,9 @@ #include <navgraph/navgraph.h> #include <plugins/skiller-simulator/execution_time_estimator.h> +#include <string> +#include <vector> + namespace fawkes { namespace skiller_simulator { class NavGraphEstimator : public ExecutionTimeEstimator @@ -37,10 +40,11 @@ public: void execute(const Skill &skill) override; private: - LockPtr<NavGraph> navgraph_; - float last_pose_x_; - float last_pose_y_; - float speed_; + LockPtr<NavGraph> navgraph_; + std::vector<std::string> skills_; + float last_pose_x_; + float last_pose_y_; + float speed_; }; } // namespace skiller_simulator } // namespace fawkes - *commit* 8162992439b35063e423bde912ce38f10d489839 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 13:04:27 2020 +0100 Subject: skiller-simulator: use config prefix 'skiller-simulator' src/plugins/skiller-simulator/estimators/config_estimator.h | 2 +- src/plugins/skiller-simulator/exec_thread.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator/estimators/config_estimator.h b/src/plugins/skiller-simulator/estimators/config_estimator.h index 46fe4d72e..b17f48bef 100644 --- a/src/plugins/skiller-simulator/estimators/config_estimator.h +++ b/src/plugins/skiller-simulator/estimators/config_estimator.h @@ -35,7 +35,7 @@ public: private: Configuration *const config_; - constexpr static char cfg_prefix_[] = "/plugins/skiller-simulation/execution-times/"; + constexpr static char cfg_prefix_[] = "/plugins/skiller-simulator/execution-times/"; }; } // namespace skiller_simulator diff --git a/src/plugins/skiller-simulator/exec_thread.cpp b/src/plugins/skiller-simulator/exec_thread.cpp index d10f350cd..aaa8e1a39 100644 --- a/src/plugins/skiller-simulator/exec_thread.cpp +++ b/src/plugins/skiller-simulator/exec_thread.cpp @@ -47,7 +47,7 @@ SkillerSimulatorExecutionThread::init() { skiller_if_ = blackboard->open_for_writing<SkillerInterface>("Skiller"); default_skill_runtime_ = - config->get_float_or_default("/plugins/skiller-simulation/execution-times/default", 1); + config->get_float_or_default("/plugins/skiller-simulator/execution-times/default", 1); skill_starttime_ = Time(); execution_time_estimator_manager_.register_provider( std::make_shared<skiller_simulator::ConfigExecutionTimeEstimator>(config)); - *commit* ddc43282c88d3ef089be0ea38548e80fe7bceaa0 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 13:05:52 2020 +0100 Subject: skiller-simulator-navgraph: use config prefix 'skiller-simulator' src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) _Diff for modified files_: diff --git a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp index 5d9bcadfd..0e574bf1a 100644 --- a/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp +++ b/src/plugins/skiller-simulator-navgraph/navgraph_estimator.cpp @@ -37,9 +37,8 @@ NavGraphEstimator::NavGraphEstimator(LockPtr<NavGraph> navgraph, Configuration * { last_pose_x_ = config->get_float_or_default("plugins/amcl/init_pose_x", 0); last_pose_y_ = config->get_float_or_default("plugins/amcl/init_pose_y", 0); - speed_ = - config->get_float_or_default("plugins/skiller-simulation/estimators/navgraph/speed", 0.5); - skills_ = config->get_strings_or_defaults("plugins/skiller-simulation/estimators/navgraph/skills", + speed_ = config->get_float_or_default("plugins/skiller-simulator/estimators/navgraph/speed", 0.5); + skills_ = config->get_strings_or_defaults("plugins/skiller-simulator/estimators/navgraph/skills", {"goto"}); } - *commit* 2b32d12e18ae42c91a94b6190731debe06168c9e - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Thu Jan 9 13:10:16 2020 +0100 Subject: config: add default config for the skiller-simulator cfg/conf.d/skiller-simulator.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) _______________________________________________ fawkes-commits mailing list fawkes-commits@lists.kbsg.rwth-aachen.de https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
