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 (skiller_if_->exclusive_controller() == 0) {
59  "%s is new exclusive controller (ID %u)",
60  m->sender_thread_name(),
61  m->sender_id());
62  skiller_if_->set_exclusive_controller(m->sender_id());
63  write_interface = true;
64  } else if (m->is_steal_control()) {
65  logger->log_warn(name(), "%s steals exclusive control", m->sender_thread_name());
66  skiller_if_->set_exclusive_controller(m->sender_id());
67  skiller_if_->set_msgid(m->id());
68  write_interface = true;
69  } else {
71  name(),
72  "%s tried to acquire exclusive control, but another controller exists already",
73  m->sender_thread_name());
74  }
75  } else if (skiller_if_->msgq_first_is<SkillerInterface::ReleaseControlMessage>()) {
78  if (skiller_if_->exclusive_controller() == m->sender_id()) {
79  logger->log_debug(name(), "%s releases exclusive control", m->sender_thread_name());
80  } else if (skiller_if_->exclusive_controller() != 0) {
81  logger->log_warn(name(),
82  "%s tried to release exclusive control, but it's not the controller",
83  m->sender_thread_name());
84  }
85  } else if (skiller_if_->msgq_first_is<SkillerInterface::ExecSkillMessage>()) {
88  if (skiller_if_->exclusive_controller() == 0
89  || skiller_if_->exclusive_controller() == m->sender_id()) {
90  if (skill_enqueued) {
91  logger->log_warn(name(),
92  "More than one skill string enqueued, ignoring previous string (%s).",
93  skiller_if_->skill_string());
94  }
95  if (skiller_if_->exclusive_controller() == 0) {
96  std::string sender = m->sender_thread_name();
97  if (sender == "Unknown") {
98  sender = "Remote";
99  }
100  logger->log_info(name(),
101  "%s executed '%s' without any exclusive controller",
102  sender.c_str(),
103  m->skill_string());
104  } else {
105  logger->log_info(name(), "%s executes '%s'", m->sender_thread_name(), m->skill_string());
106  }
107  }
108  if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
109  logger->log_info(name(),
110  "Aborting execution of previous skill string '%s' for new goal",
111  skiller_if_->skill_string());
112  }
113  skiller_if_->set_skill_string(m->skill_string());
114  skiller_if_->set_msgid(m->id());
115  skiller_if_->set_error("");
116  skiller_if_->set_status(SkillerInterface::S_RUNNING);
117  current_skill_runtime_ = get_skill_runtime(m->skill_string());
118  logger->log_info(name(),
119  "Executing '%s', will take %.2f seconds",
120  m->skill_string(),
121  current_skill_runtime_);
122  skill_starttime_ = Time();
123  write_interface = true;
124  skill_enqueued = true;
125  } else if (skiller_if_->msgq_first_is<SkillerInterface::StopExecMessage>()) {
128  if (skiller_if_->exclusive_controller() == m->sender_id()) {
129  logger->log_debug(name(),
130  "Stopping execution of '%s' on request",
131  skiller_if_->skill_string());
132  skiller_if_->set_skill_string("");
133  skiller_if_->set_error("");
134  skiller_if_->set_msgid(m->id());
135  skiller_if_->set_status(SkillerInterface::S_INACTIVE);
136  } else {
137  std::string sender = m->sender_thread_name();
138  if (sender == "Unknown") {
139  sender = "Remote";
140  }
141  logger->log_debug(name(), "%s sent stop without any exclusive controller", sender.c_str());
142  }
143  } else {
144  logger->log_warn(name(), "Unhandled message in skiller interface");
145  }
146  skiller_if_->msgq_pop();
147  }
148 
149  if (!skill_enqueued) {
150  if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
151  Time now = Time();
152  if (Time() > skill_starttime_ + current_skill_runtime_) {
153  logger->log_info(name(), "Skill '%s' is final", skiller_if_->skill_string());
154  auto [exec_status, error] = execute_skill(skiller_if_->skill_string());
155  skiller_if_->set_skill_string(skiller_if_->skill_string());
156  skiller_if_->set_error(error.c_str());
157  skiller_if_->set_status(exec_status);
158  write_interface = true;
159  }
160  }
161  }
162 
163  if (write_interface) {
164  skiller_if_->write();
165  }
166 }
167 
168 void
170 {
171  blackboard->close(skiller_if_);
172 }
173 
174 float
175 SkillerSimulatorExecutionThread::get_skill_runtime(const std::string &skill) const
176 {
177  auto provider = execution_time_estimator_manager_->get_provider(skill);
178  return provider->get_execution_time(skill);
179 }
180 
181 std::pair<fawkes::SkillerInterface::SkillStatusEnum, std::string>
182 SkillerSimulatorExecutionThread::execute_skill(const std::string &skill)
183 {
184  auto provider = execution_time_estimator_manager_->get_provider(skill);
185  return provider->execute(skill);
186 }
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
fawkes::SkillerInterface::set_exclusive_controller
void set_exclusive_controller(const uint32_t new_exclusive_controller)
Set exclusive_controller value.
Definition: SkillerInterface.cpp:198
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
SkillerSimulatorExecutionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: exec_thread.cpp:49
fawkes::SkillerInterface::AcquireControlMessage::is_steal_control
bool is_steal_control() const
Get steal_control value.
Definition: SkillerInterface.cpp:591
fawkes::SkillerInterface::ReleaseControlMessage
ReleaseControlMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:186
SkillerSimulatorExecutionThread::SkillerSimulatorExecutionThread
SkillerSimulatorExecutionThread()
Constructor.
Definition: exec_thread.cpp:35
fawkes::SkillerInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: SkillerInterface.cpp:234
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
fawkes::Message::sender_thread_name
const char * sender_thread_name() const
Get sender of message.
Definition: message.cpp:307
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Message::sender_id
unsigned int sender_id() const
Get ID of sender.
Definition: message.cpp:316
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ExecutionTimeEstimatorsAspect::execution_time_estimator_manager_
ExecutionTimeEstimatorManager * execution_time_estimator_manager_
The ExecutionTimeEstimatorManager that is used to manage the estimators.
Definition: execution_time_estimator.h:51
fawkes::ExecutionTimeEstimatorManager::get_provider
std::shared_ptr< ExecutionTimeEstimator > get_provider(const std::string &skill_string) const
Get the execution time provider for the given skill string.
Definition: execution_time_estimator.cpp:42
fawkes::SkillerInterface::AcquireControlMessage
AcquireControlMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:155
fawkes::SkillerInterface::set_error
void set_error(const char *new_error)
Set error value.
Definition: SkillerInterface.cpp:160
fawkes::SkillerInterface::ExecSkillMessage
ExecSkillMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:84
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::SkillerInterface::StopExecMessage
StopExecMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:134
fawkes::SkillerInterface::set_status
void set_status(const SkillStatusEnum new_status)
Set status value.
Definition: SkillerInterface.cpp:268
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes
Fawkes library namespace.
fawkes::SkillerInterface::status
SkillStatusEnum status() const
Get status value.
Definition: SkillerInterface.cpp:246
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::SkillerInterface
SkillerInterface Fawkes BlackBoard Interface.
Definition: SkillerInterface.h:34
fawkes::SkillerInterface::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:103
fawkes::SkillerInterface::ExecSkillMessage::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:385
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::SkillerInterface::set_skill_string
void set_skill_string(const char *new_skill_string)
Set skill_string value.
Definition: SkillerInterface.cpp:126
SkillerSimulatorExecutionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: exec_thread.cpp:169
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:180
fawkes::SkillerInterface::exclusive_controller
uint32_t exclusive_controller() const
Get exclusive_controller value.
Definition: SkillerInterface.cpp:174
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
SkillerSimulatorExecutionThread::init
virtual void init()
Initialize the thread.
Definition: exec_thread.cpp:42