Fawkes API Fawkes Development Version
exec_thread.cpp
1/***************************************************************************
2 * exec_thread.cpp - Simulate skill execution
3 *
4 * Created: Mon 06 May 2019 08:51:53 CEST 08:51
5 * Copyright 2019 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
6 ****************************************************************************/
7
8/* This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2 of the License, or
11 * (at your option) any later version.
12 *
13 * This program is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Library General Public License for more details.
17 *
18 * Read the full text in the LICENSE.GPL file in the doc directory.
19 */
20
21#include "exec_thread.h"
22
23using namespace std;
24using namespace fawkes;
25
26/** @class SkillerSimulatorExecutionThread "exec_thread.h"
27 * Simulated Skill Execution Thread.
28 * This thread pretends to execute a skill, similar to the real skiller execution thread.
29 *
30 * @see SkillerExecutionThread
31 * @author Till Hofmann
32 */
33
34/** Constructor. */
36: Thread("SkillerSimulatorExecutionThread", Thread::OPMODE_WAITFORWAKEUP),
38{
39}
40
41void
43{
44 skiller_if_ = blackboard->open_for_writing<SkillerInterface>("Skiller");
45 skill_starttime_ = Time();
46}
47
48void
50{
51 bool skill_enqueued = false;
52 bool write_interface = false;
53 while (!skiller_if_->msgq_empty()) {
57 if (strcmp(skiller_if_->exclusive_controller(), "") == 0) {
58 const std::string new_controller = m->source_id().get_string();
60 "%s is new exclusive controller (ID %s)",
62 new_controller.c_str());
63 skiller_if_->set_exclusive_controller(new_controller.c_str());
64 skiller_if_->set_msgid(m->id());
65 write_interface = true;
66 } else if (m->is_steal_control()) {
67 const std::string new_controller = m->source_id().get_string();
69 "%s steals exclusive control (ID %s)",
71 new_controller.c_str());
72 skiller_if_->set_exclusive_controller(new_controller.c_str());
73 skiller_if_->set_msgid(m->id());
74 write_interface = true;
75 } else {
77 name(),
78 "%s tried to acquire exclusive control, but another controller exists already",
80 }
81 } else if (skiller_if_->msgq_first_is<SkillerInterface::ReleaseControlMessage>()) {
84 if (skiller_if_->exclusive_controller() == m->source_id().get_string()) {
85 logger->log_debug(name(), "%s releases exclusive control", m->sender_thread_name());
86 } else if (strcmp(skiller_if_->exclusive_controller(), "") != 0) {
88 "%s tried to release exclusive control, but it's not the controller",
90 }
91 } else if (skiller_if_->msgq_first_is<SkillerInterface::ExecSkillMessage>()) {
94 if (strcmp(skiller_if_->exclusive_controller(), "") == 0
95 || skiller_if_->exclusive_controller() == m->source_id().get_string()) {
96 if (skill_enqueued) {
98 "More than one skill string enqueued, ignoring previous string (%s).",
99 skiller_if_->skill_string());
100 }
101 if (strcmp(skiller_if_->exclusive_controller(), "") == 0) {
102 std::string sender = m->sender_thread_name();
103 if (sender == "Unknown") {
104 sender = "Remote";
105 }
107 "%s executed '%s' without any exclusive controller",
108 sender.c_str(),
109 m->skill_string());
110 } else {
111 logger->log_info(name(), "%s executes '%s'", m->sender_thread_name(), m->skill_string());
112 }
113 }
114 if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
116 "Aborting execution of previous skill string '%s' for new goal",
117 skiller_if_->skill_string());
118 }
119 skiller_if_->set_skill_string(m->skill_string());
120 skiller_if_->set_msgid(m->id());
121 skiller_if_->set_error("");
122 skiller_if_->set_status(SkillerInterface::S_RUNNING);
123 current_skill_runtime_ = get_skill_runtime(m->skill_string());
125 "Executing '%s', will take %.2f seconds",
126 m->skill_string(),
127 current_skill_runtime_);
128 skill_starttime_ = Time();
129 write_interface = true;
130 skill_enqueued = true;
131 } else if (skiller_if_->msgq_first_is<SkillerInterface::StopExecMessage>()) {
134 if (skiller_if_->exclusive_controller() == m->source_id().get_string()) {
136 "Stopping execution of '%s' on request",
137 skiller_if_->skill_string());
138 skiller_if_->set_skill_string("");
139 skiller_if_->set_error("");
140 skiller_if_->set_msgid(m->id());
141 skiller_if_->set_status(SkillerInterface::S_INACTIVE);
142 } else {
143 std::string sender = m->sender_thread_name();
144 if (sender == "Unknown") {
145 sender = "Remote";
146 }
147 logger->log_debug(name(), "%s sent stop without any exclusive controller", sender.c_str());
148 }
149 } else {
150 logger->log_warn(name(), "Unhandled message in skiller interface");
151 }
152 skiller_if_->msgq_pop();
153 }
154
155 if (!skill_enqueued) {
156 if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
157 Time now = Time();
158 if (Time() > skill_starttime_ + current_skill_runtime_) {
159 logger->log_info(name(), "Skill '%s' is final", skiller_if_->skill_string());
160 auto [exec_status, error] = execute_skill(skiller_if_->skill_string());
161 skiller_if_->set_skill_string(skiller_if_->skill_string());
162 skiller_if_->set_error(error.c_str());
163 skiller_if_->set_status(exec_status);
164 write_interface = true;
165 }
166 }
167 }
168
169 if (write_interface) {
170 skiller_if_->write();
171 }
172}
173
174void
176{
177 blackboard->close(skiller_if_);
178}
179
180float
181SkillerSimulatorExecutionThread::get_skill_runtime(const std::string &skill) const
182{
183 auto provider = execution_time_estimator_manager_->get_provider(skill);
184 return provider->get_execution_time(skill);
185}
186
187std::pair<fawkes::SkillerInterface::SkillStatusEnum, std::string>
188SkillerSimulatorExecutionThread::execute_skill(const std::string &skill)
189{
190 auto provider = execution_time_estimator_manager_->get_provider(skill);
191 return provider->execute(skill);
192}
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
Definition: exec_thread.cpp:42
SkillerSimulatorExecutionThread()
Constructor.
Definition: exec_thread.cpp:35
virtual void loop()
Code to execute in the thread.
Definition: exec_thread.cpp:49
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
std::shared_ptr< ExecutionTimeEstimator > get_provider(const std::string &skill_string) const
Get the execution time provider for the given skill string.
ExecutionTimeEstimatorManager * execution_time_estimator_manager_
The ExecutionTimeEstimatorManager that is used to manage the estimators.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Uuid source_id() const
Get ID of the original source of the message.
Definition: message.cpp:346
const char * sender_thread_name() const
Get sender of message.
Definition: message.cpp:327
unsigned int id() const
Get message ID.
Definition: message.cpp:181
AcquireControlMessage Fawkes BlackBoard Interface Message.
bool is_steal_control() const
Get steal_control value.
ExecSkillMessage Fawkes BlackBoard Interface Message.
char * skill_string() const
Get skill_string value.
ReleaseControlMessage Fawkes BlackBoard Interface Message.
StopExecMessage Fawkes BlackBoard Interface Message.
SkillerInterface Fawkes BlackBoard Interface.
void set_skill_string(const char *new_skill_string)
Set skill_string value.
void set_error(const char *new_error)
Set error value.
SkillStatusEnum status() const
Get status value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_status(const SkillStatusEnum new_status)
Set status value.
void set_exclusive_controller(const char *new_exclusive_controller)
Set exclusive_controller value.
char * skill_string() const
Get skill_string value.
char * exclusive_controller() const
Get exclusive_controller value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
std::string get_string() const
Get the string representation of the Uuid.
Definition: uuid.cpp:107
Fawkes library namespace.