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 
23 using namespace std;
24 using 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),
37  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SKILL)
38 {
39 }
40 
41 void
43 {
44  skiller_if_ = blackboard->open_for_writing<SkillerInterface>("Skiller");
45  skill_starttime_ = Time();
46 }
47 
48 void
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)",
61  m->sender_thread_name(),
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();
68  logger->log_warn(name(),
69  "%s steals exclusive control (ID %s)",
70  m->sender_thread_name(),
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",
79  m->sender_thread_name());
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) {
87  logger->log_warn(name(),
88  "%s tried to release exclusive control, but it's not the controller",
89  m->sender_thread_name());
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) {
97  logger->log_warn(name(),
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  }
106  logger->log_info(name(),
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) {
115  logger->log_info(name(),
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());
124  logger->log_info(name(),
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()) {
135  logger->log_debug(name(),
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 
174 void
176 {
177  blackboard->close(skiller_if_);
178 }
179 
180 float
181 SkillerSimulatorExecutionThread::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 
187 std::pair<fawkes::SkillerInterface::SkillStatusEnum, std::string>
188 SkillerSimulatorExecutionThread::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.