Fawkes API  Fawkes Development Version
be_adapter.cpp
1 
2 /***************************************************************************
3  * be_adapter.cpp - PLEXIL adapter for the Behavior Engine
4  *
5  * Created: Tue Aug 14 15:23:21 2018
6  * Copyright 2006-2018 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "be_adapter.h"
23 
24 #include <utils/misc/map_skill.h>
25 
26 #include <AdapterConfiguration.hh>
27 #include <AdapterExecInterface.hh>
28 #include <AdapterFactory.hh>
29 #include <Command.hh>
30 #include <algorithm>
31 
32 using namespace fawkes;
33 
34 /** @class BehaviorEnginePlexilAdapter "be_adapter.h"
35  * Plexil adapter to provide access to the Behavior Engine.
36  * @author Tim Niemueller
37  */
38 
39 /** Constructor.
40  * @param execInterface Reference to the parent AdapterExecInterface object.
41  */
43  PLEXIL::AdapterExecInterface &execInterface)
44 : InterfaceAdapter(execInterface), BlackBoardInterfaceListener("PlexilBE")
45 {
46 }
47 
48 /** Constructor from configuration XML.
49  * @param execInterface Reference to the parent AdapterExecInterface object.
50  * @param xml A const reference to the XML element describing this adapter
51  * @note The instance maintains a shared pointer to the XML.
52  */
54  PLEXIL::AdapterExecInterface &execInterface,
55  pugi::xml_node const xml)
56 : InterfaceAdapter(execInterface, xml), BlackBoardInterfaceListener("PlexilBE")
57 {
58 }
59 
60 /** Destructor. */
62 {
63 }
64 
65 /** Initialize adapter.
66  * @return true if initialization was successful, false otherwise.
67  */
68 bool
70 {
71  logger_ = reinterpret_cast<fawkes::Logger *>(m_execInterface.getProperty("::Fawkes::Logger"));
72  blackboard_ =
73  reinterpret_cast<fawkes::BlackBoard *>(m_execInterface.getProperty("::Fawkes::BlackBoard"));
74  config_ =
75  reinterpret_cast<fawkes::Configuration *>(m_execInterface.getProperty("::Fawkes::Config"));
76 
77  std::string cfg_prefix;
78  try {
79  std::string cfg_spec = config_->get_string("/plexil/spec");
80  cfg_prefix = "/plexil/" + cfg_spec + "/";
81  } catch (fawkes::Exception &e) {
82  logger_->log_error("PlexilBE", "Failed to read config: %s", e.what_no_backtrace());
83  return false;
84  }
85 
86  skill_msgid_ = 0;
87  current_cmd_ = nullptr;
88 
89  // Parse adapter configurations
90  std::string skills_config_prefix = cfg_prefix + "skills/";
91  std::unique_ptr<Configuration::ValueIterator> cfg_item{config_->search(skills_config_prefix)};
92  while (cfg_item->next()) {
93  std::string path = cfg_item->path();
94 
95  std::string::size_type start_pos = skills_config_prefix.size();
96  std::string::size_type slash_pos = path.find("/", start_pos + 1);
97  if (slash_pos != std::string::npos) {
98  std::string id = path.substr(start_pos, slash_pos - start_pos);
99 
100  start_pos = slash_pos + 1;
101  slash_pos = path.find("/", start_pos);
102  std::string what = path.substr(start_pos, slash_pos - start_pos);
103 
104  if (what == "name") {
105  cfg_skills_[id].name = cfg_item->get_string();
106  } else if (what == "template") {
107  cfg_skills_[id].template_str = cfg_item->get_string();
108  } else if (what == "args") {
109  start_pos = slash_pos + 1;
110  slash_pos = path.find("/", start_pos);
111  size_t args_id = stoi(path.substr(start_pos, slash_pos - start_pos));
112 
113  // since we do get the values in order, this keeps or grows size only
114  cfg_skills_[id].args.resize(args_id + 1);
115 
116  start_pos = slash_pos + 1;
117  slash_pos = path.find("/", start_pos);
118  std::string args_what = path.substr(start_pos, slash_pos - start_pos);
119 
120  if (args_what == "type") {
121  std::string type_str = cfg_item->get_as_string();
122  cfg_skills_[id].args[args_id].type = PLEXIL::UNKNOWN_TYPE;
123  if (type_str == "String") {
124  cfg_skills_[id].args[args_id].type = PLEXIL::STRING_TYPE;
125  } else if (type_str == "Integer") {
126  cfg_skills_[id].args[args_id].type = PLEXIL::INTEGER_TYPE;
127  } else if (type_str == "Real") {
128  cfg_skills_[id].args[args_id].type = PLEXIL::REAL_TYPE;
129  } else if (type_str == "Boolean") {
130  cfg_skills_[id].args[args_id].type = PLEXIL::BOOLEAN_TYPE;
131  } else {
132  logger_->log_warn("PlexilBE",
133  "Invalid argument type '%s' for '%s'",
134  type_str.c_str(),
135  cfg_skills_[id].name.c_str());
136  }
137  } else if (args_what == "name") {
138  cfg_skills_[id].args[args_id].name = cfg_item->get_as_string();
139  }
140  }
141  }
142  }
143 
144  PLEXIL::g_configuration->registerCommandInterface("skill_call", this);
145 
146  std::map<std::string, std::string> mapping;
147 
148  logger_->log_debug("PlexilBE", "Skills");
149  for (const auto &skill_entry : cfg_skills_) {
150  const auto &skill = skill_entry.second;
151  std::string line = "- " + skill.name + " (";
152  bool first = true;
153  for (const auto &arg : skill.args) {
154  if (!first) {
155  line += ", ";
156  } else {
157  first = false;
158  }
159  line += PLEXIL::valueTypeName(arg.type) + " " + arg.name;
160  }
161  line += ") -> " + skill.template_str;
162  logger_->log_debug("PlexilBE", "%s", line.c_str());
163 
164  mapping[skill.name] = skill.template_str;
165  PLEXIL::g_configuration->registerCommandInterface(skill.name, this);
166  }
167 
168  action_skill_mapping_ = std::make_shared<fawkes::ActionSkillMapping>(mapping);
169 
170  return true;
171 }
172 
173 /** Start adapter.
174  * @return true if starting was successful, false otherwise.
175  */
176 bool
178 {
179  try {
180  skiller_if_ = blackboard_->open_for_reading<SkillerInterface>("Skiller");
181 
182  bbil_add_data_interface(skiller_if_);
183  blackboard_->register_listener(this, BlackBoard::BBIL_FLAG_DATA);
184  } catch (Exception &e) {
185  logger_->log_error("PlexilBE", "Failed to open skiller interface: %s", e.what_no_backtrace());
186  return false;
187  }
188 
189  skiller_if_->read();
190  if (!skiller_if_->has_writer()) {
191  logger_->log_error("PlexilBE", "No writer for skiller interface");
192  return false;
193  }
194  if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
196  new SkillerInterface::AcquireControlMessage(/* steal control */ true);
197  skiller_if_->msgq_enqueue(msg);
198  }
199  return true;
200 }
201 
202 /** Stop adapter.
203  * @return true if successful, false otherwise.
204  */
205 bool
207 {
208  return true;
209 }
210 
211 /** Reset adapter.
212  * @return true if successful, false otherwise.
213  */
214 bool
216 {
217  return true;
218 }
219 
220 /** Shut adapter down.
221  * @return true if successful, false otherwise.
222  */
223 bool
225 {
226  blackboard_->unregister_listener(this);
227  blackboard_->close(skiller_if_);
228  return true;
229 }
230 
231 std::string
232 BehaviorEnginePlexilAdapter::format_skillstring(const std::vector<PLEXIL::Value> &values)
233 {
234  std::string rv;
235  if (values.size() % 2 == 0) {
236  logger_->log_warn("PlexilBE",
237  "Malformed skill call, must be 'skillname argname0 argvalue1...'");
238  } else if (values.size() > 0) {
239  rv = values[0].valueToString() + "{";
240  bool first = true;
241  for (size_t i = 1; i < values.size() - 1; i += 2) {
242  if (!first) {
243  rv += ", ";
244  } else {
245  first = false;
246  }
247 
248  rv += values[i].valueToString() + "=";
249  if (values[i + 1].valueType() == PLEXIL::STRING_TYPE) {
250  rv += "\"" + values[i + 1].valueToString() + "\"";
251  } else {
252  rv += values[i + 1].valueToString();
253  }
254  }
255  rv += "}";
256  }
257 
258  return rv;
259 }
260 
261 std::string
262 BehaviorEnginePlexilAdapter::map_skillstring(const std::string & name,
263  const skill_config & skill_config,
264  const std::vector<PLEXIL::Value> &values)
265 {
266  if (skill_config.args.size() != values.size()) {
267  logger_->log_warn("PlexilBE",
268  "Arguments for '%s' do not match spec (got %zu, expected %zu)",
269  name.c_str(),
270  skill_config.args.size(),
271  values.size());
272  return "";
273  }
274  for (size_t i = 0; i < skill_config.args.size(); ++i) {
275  if (skill_config.args[i].type != values[i].valueType()) {
276  logger_->log_warn("PlexilBE",
277  "Arguments type mismatch for '%s' of '%s' (got %s, expected %s)",
278  skill_config.args[i].name.c_str(),
279  name.c_str(),
280  PLEXIL::valueTypeName(values[i].valueType()).c_str(),
281  PLEXIL::valueTypeName(skill_config.args[i].type).c_str());
282  return "";
283  }
284  }
285  if (!action_skill_mapping_->has_mapping(name)) {
286  logger_->log_warn("PlexilBE", "No mapping for action '%s' known", name.c_str());
287  return "";
288  }
289 
290  std::map<std::string, std::string> param_map;
291  for (size_t i = 0; i < skill_config.args.size(); ++i) {
292  param_map[skill_config.args[i].name] = values[i].valueToString();
293  }
294 
295  std::multimap<std::string, std::string> messages;
296  std::string rv = action_skill_mapping_->map_skill(name, param_map, messages);
297  for (auto &m : messages) {
298  if (m.first == "WARN") {
299  logger_->log_warn("PlexilBE", "%s", m.second.c_str());
300  } else if (m.first == "ERROR") {
301  logger_->log_error("PlexilBE", "%s", m.second.c_str());
302  } else if (m.first == "DEBUG") {
303  logger_->log_debug("PlexilBE", "%s", m.second.c_str());
304  } else {
305  logger_->log_info("PlexilBE", "%s", m.second.c_str());
306  }
307  }
308  return rv;
309 }
310 
311 void
312 BehaviorEnginePlexilAdapter::call_skill(const std::string &skill_string, PLEXIL::Command *cmd)
313 {
314  logger_->log_info("PlexilBE", "Executing skill '%s'", skill_string.c_str());
316  new SkillerInterface::ExecSkillMessage(skill_string.c_str());
317  msg->ref();
318 
319  skiller_if_->msgq_enqueue(msg);
320 
321  skill_msgid_ = msg->id();
322  skill_string_ = skill_string;
323  current_cmd_ = cmd;
324 
325  msg->unref();
326 }
327 
328 /** Perform given command.
329  * @param cmd command to execute
330  */
331 void
333 {
334  std::lock_guard<std::mutex> lock(exec_mutex_);
335 
336  if (cmd->getName() == "skill_call") {
337  std::string skill_string = format_skillstring(cmd->getArgValues());
338  call_skill(skill_string, cmd);
339  m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SENT_TO_SYSTEM);
340  } else {
341  std::string name = cmd->getName();
342  auto skill_entry = std::find_if(cfg_skills_.begin(), cfg_skills_.end(), [&name](const auto &e) {
343  return e.second.name == name;
344  });
345  if (skill_entry != cfg_skills_.end()) {
346  std::string skill_string = map_skillstring(name, skill_entry->second, cmd->getArgValues());
347  if (!skill_string.empty()) {
348  call_skill(skill_string, cmd);
349  m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SENT_TO_SYSTEM);
350  } else {
351  m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
352  }
353  } else {
354  logger_->log_warn("PlexilBE", "Called for unknown skill '%s'", name.c_str());
355  m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
356  }
357  }
358 
359  m_execInterface.notifyOfExternalEvent();
360 }
361 
362 /** Abort currently running execution.
363  * @param cmd command to abort
364  */
365 void
367 {
368  logger_->log_warn("PlexilBE", "Aborting %s", cmd->getName().c_str());
369  if (current_cmd_) {
370  try {
372  } catch (Exception &e) {
373  }
374  current_cmd_ = nullptr;
375  m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
376  m_execInterface.handleCommandAbortAck(cmd, false);
377  m_execInterface.notifyOfExternalEvent();
378  }
379 }
380 
381 void
383 {
384  std::lock_guard<std::mutex> lock(exec_mutex_);
385  skiller_if_->read();
386  if (current_cmd_) {
387  if (skiller_if_->msgid() == skill_msgid_) {
388  switch (skiller_if_->status()) {
389  case SkillerInterface::S_FINAL:
390  m_execInterface.handleCommandReturn(current_cmd_, PLEXIL::Value(true));
391  m_execInterface.handleCommandAck(current_cmd_, PLEXIL::COMMAND_SUCCESS);
392  m_execInterface.notifyOfExternalEvent();
393  current_cmd_ = nullptr;
394  break;
395  case SkillerInterface::S_FAILED:
396  m_execInterface.handleCommandReturn(current_cmd_, PLEXIL::Value(false));
397  m_execInterface.handleCommandAck(current_cmd_, PLEXIL::COMMAND_FAILED);
398  m_execInterface.notifyOfExternalEvent();
399  current_cmd_ = nullptr;
400  break;
401  default:
402  if (current_cmd_->getCommandHandle() == PLEXIL::COMMAND_SENT_TO_SYSTEM) {
403  m_execInterface.handleCommandAck(current_cmd_, PLEXIL::COMMAND_RCVD_BY_SYSTEM);
404  }
405  }
406  }
407  }
408 }
409 
410 extern "C" {
411 void
412 initBehaviorEngineAdapter()
413 {
414  REGISTER_ADAPTER(BehaviorEnginePlexilAdapter, "BehaviorEngineAdapter");
415 }
416 }
fawkes::RefCount::unref
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:99
BehaviorEnginePlexilAdapter::invokeAbort
void invokeAbort(PLEXIL::Command *cmd)
Abort currently running execution.
Definition: be_adapter.cpp:366
BehaviorEnginePlexilAdapter::stop
virtual bool stop()
Stop adapter.
Definition: be_adapter.cpp:206
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:188
BehaviorEnginePlexilAdapter
Interface adapter to provide logging facilities.
Definition: be_adapter.h:39
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:475
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:215
fawkes::BlackBoardInterfaceListener
Definition: interface_listener.h:45
BehaviorEnginePlexilAdapter::shutdown
virtual bool shutdown()
Shut adapter down.
Definition: be_adapter.cpp:224
fawkes::SkillerInterface::AcquireControlMessage
Definition: SkillerInterface.h:163
fawkes::SkillerInterface::ExecSkillMessage
Definition: SkillerInterface.h:92
fawkes::SkillerInterface::StopExecMessage
Definition: SkillerInterface.h:142
fawkes::RefCount::ref
void ref()
Increment reference count.
Definition: refcount.cpp:71
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
BehaviorEnginePlexilAdapter::initialize
virtual bool initialize()
Initialize adapter.
Definition: be_adapter.cpp:69
BehaviorEnginePlexilAdapter::start
virtual bool start()
Start adapter.
Definition: be_adapter.cpp:177
fawkes
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Interface
Definition: interface.h:77
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
BehaviorEnginePlexilAdapter::~BehaviorEnginePlexilAdapter
virtual ~BehaviorEnginePlexilAdapter()
Destructor.
Definition: be_adapter.cpp:61
fawkes::SkillerInterface
Definition: SkillerInterface.h:37
BehaviorEnginePlexilAdapter::bb_interface_data_changed
virtual void bb_interface_data_changed(fawkes::Interface *interface)
BlackBoard data changed notification.
Definition: be_adapter.cpp:382
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:662
fawkes::Interface::serial
unsigned short serial() const
Get instance serial of interface.
Definition: interface.cpp:686
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:184
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
fawkes::SkillerInterface::exclusive_controller
uint32_t exclusive_controller() const
Get exclusive_controller value.
Definition: SkillerInterface.cpp:182
BehaviorEnginePlexilAdapter::reset
virtual bool reset()
Reset adapter.
Definition: be_adapter.cpp:215
fawkes::BlackBoardInterfaceListener::bbil_add_data_interface
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
Definition: interface_listener.cpp:236
BehaviorEnginePlexilAdapter::executeCommand
void executeCommand(PLEXIL::Command *cmd)
Perform given command.
Definition: be_adapter.cpp:332
BehaviorEnginePlexilAdapter::BehaviorEnginePlexilAdapter
BehaviorEnginePlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
Constructor.
Definition: be_adapter.cpp:42
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Exception
Definition: exception.h:39