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_runtime_ = config->get_float_or_default("/plugins/skiller-simulation/skill-runtime", 1);
46  skill_starttime_ = Time();
47 }
48 
49 void
51 {
52  bool skill_enqueued = false;
53  bool write_interface = false;
54  while (!skiller_if_->msgq_empty()) {
58  if (skiller_if_->exclusive_controller() == 0) {
60  "%s is new exclusive controller (ID %u)",
61  m->sender_thread_name(),
62  m->sender_id());
63  skiller_if_->set_exclusive_controller(m->sender_id());
64  write_interface = true;
65  } else if (m->is_steal_control()) {
66  logger->log_warn(name(), "%s steals exclusive control", m->sender_thread_name());
67  skiller_if_->set_exclusive_controller(m->sender_id());
68  skiller_if_->set_msgid(m->id());
69  write_interface = true;
70  } else {
72  name(),
73  "%s tried to acquire exclusive control, but another controller exists already",
74  m->sender_thread_name());
75  }
76  } else if (skiller_if_->msgq_first_is<SkillerInterface::ReleaseControlMessage>()) {
79  if (skiller_if_->exclusive_controller() == m->sender_id()) {
80  logger->log_debug(name(), "%s releases exclusive control", m->sender_thread_name());
81  } else if (skiller_if_->exclusive_controller() != 0) {
82  logger->log_warn(name(),
83  "%s tried to release exclusive control, but it's not the controller",
84  m->sender_thread_name());
85  }
86  } else if (skiller_if_->msgq_first_is<SkillerInterface::ExecSkillMessage>()) {
89  if (skiller_if_->exclusive_controller() == 0
90  || skiller_if_->exclusive_controller() == m->sender_id()) {
91  if (skill_enqueued) {
92  logger->log_warn(name(),
93  "More than one skill string enqueued, ignoring previous string (%s).",
94  skiller_if_->skill_string());
95  }
96  if (skiller_if_->exclusive_controller() == 0) {
97  std::string sender = m->sender_thread_name();
98  if (sender == "Unknown") {
99  sender = "Remote";
100  }
101  logger->log_info(name(),
102  "%s executed '%s' without any exclusive controller",
103  sender.c_str(),
104  m->skill_string());
105  } else {
106  logger->log_info(name(), "%s executes '%s'", m->sender_thread_name(), m->skill_string());
107  }
108  }
109  if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
110  logger->log_info(name(),
111  "Aborting execution of previous skill string '%s' for new goal",
112  skiller_if_->skill_string());
113  }
114  skiller_if_->set_skill_string(m->skill_string());
115  skiller_if_->set_msgid(m->id());
116  skiller_if_->set_error("");
117  skiller_if_->set_status(SkillerInterface::S_RUNNING);
118  skill_starttime_ = Time();
119  write_interface = true;
120  skill_enqueued = true;
121  } else if (skiller_if_->msgq_first_is<SkillerInterface::StopExecMessage>()) {
124  if (skiller_if_->exclusive_controller() == m->sender_id()) {
125  logger->log_debug(name(),
126  "Stopping execution of '%s' on request",
127  skiller_if_->skill_string());
128  skiller_if_->set_skill_string("");
129  skiller_if_->set_error("");
130  skiller_if_->set_msgid(m->id());
131  skiller_if_->set_status(SkillerInterface::S_INACTIVE);
132  } else {
133  std::string sender = m->sender_thread_name();
134  if (sender == "Unknown") {
135  sender = "Remote";
136  }
137  logger->log_debug(name(), "%s sent stop without any exclusive controller", sender.c_str());
138  }
139  } else {
140  logger->log_warn(name(), "Unhandled message in skiller interface");
141  }
142  skiller_if_->msgq_pop();
143  }
144 
145  if (!skill_enqueued) {
146  if (skiller_if_->status() == SkillerInterface::S_RUNNING) {
147  Time now = Time();
148  if (Time() > skill_starttime_ + skill_runtime_) {
149  logger->log_info(name(), "Skill '%s' is final", skiller_if_->skill_string());
150  skiller_if_->set_skill_string("");
151  skiller_if_->set_status(SkillerInterface::S_FINAL);
152  write_interface = true;
153  }
154  }
155  }
156 
157  if (write_interface) {
158  skiller_if_->write();
159  }
160 }
161 
162 void
164 {
165  blackboard->close(skiller_if_);
166 }
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:208
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1184
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1031
SkillerSimulatorExecutionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: exec_thread.cpp:50
fawkes::SkillerInterface::AcquireControlMessage::is_steal_control
bool is_steal_control() const
Get steal_control value.
Definition: SkillerInterface.cpp:605
fawkes::Configuration::get_float_or_default
virtual float get_float_or_default(const char *path, const float &default_val)
Definition: config.cpp:702
fawkes::SkillerInterface::ReleaseControlMessage
Definition: SkillerInterface.h:197
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:245
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::Message::sender_thread_name
const char * sender_thread_name() const
Get sender of message.
Definition: message.cpp:313
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::Message::sender_id
unsigned int sender_id() const
Get ID of sender.
Definition: message.cpp:322
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::SkillerInterface::AcquireControlMessage
Definition: SkillerInterface.h:166
fawkes::SkillerInterface::set_error
void set_error(const char *new_error)
Set error value.
Definition: SkillerInterface.cpp:168
fawkes::SkillerInterface::ExecSkillMessage
Definition: SkillerInterface.h:95
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::SkillerInterface::StopExecMessage
Definition: SkillerInterface.h:145
fawkes::SkillerInterface::set_status
void set_status(const SkillStatusEnum new_status)
Set status value.
Definition: SkillerInterface.cpp:280
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes
fawkes::SkillerInterface::status
SkillStatusEnum status() const
Get status value.
Definition: SkillerInterface.cpp:258
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::SkillerInterface
Definition: SkillerInterface.h:39
fawkes::SkillerInterface::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:109
fawkes::SkillerInterface::ExecSkillMessage::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:398
fawkes::Time
Definition: time.h:98
fawkes::SkillerInterface::set_skill_string
void set_skill_string(const char *new_skill_string)
Set skill_string value.
Definition: SkillerInterface.cpp:132
SkillerSimulatorExecutionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: exec_thread.cpp:163
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:186
fawkes::SkillerInterface::exclusive_controller
uint32_t exclusive_controller() const
Get exclusive_controller value.
Definition: SkillerInterface.cpp:184
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1169
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
SkillerSimulatorExecutionThread::init
virtual void init()
Initialize the thread.
Definition: exec_thread.cpp:42