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
32using 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 */
68bool
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 */
176bool
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().get_string()) {
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 */
205bool
207{
208 return true;
209}
210
211/** Reset adapter.
212 * @return true if successful, false otherwise.
213 */
214bool
216{
217 return true;
218}
219
220/** Shut adapter down.
221 * @return true if successful, false otherwise.
222 */
223bool
225{
226 blackboard_->unregister_listener(this);
227 blackboard_->close(skiller_if_);
228 return true;
229}
230
231std::string
232BehaviorEnginePlexilAdapter::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
261std::string
262BehaviorEnginePlexilAdapter::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
311void
312BehaviorEnginePlexilAdapter::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 */
331void
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 */
365void
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
381void
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
410extern "C" {
411void
412initBehaviorEngineAdapter()
413{
414 REGISTER_ADAPTER(BehaviorEnginePlexilAdapter, "BehaviorEngineAdapter");
415}
416}
Interface adapter to provide logging facilities.
Definition: be_adapter.h:42
virtual ~BehaviorEnginePlexilAdapter()
Destructor.
Definition: be_adapter.cpp:61
void invokeAbort(PLEXIL::Command *cmd)
Abort currently running execution.
Definition: be_adapter.cpp:366
virtual bool shutdown()
Shut adapter down.
Definition: be_adapter.cpp:224
virtual bool reset()
Reset adapter.
Definition: be_adapter.cpp:215
virtual bool initialize()
Initialize adapter.
Definition: be_adapter.cpp:69
void executeCommand(PLEXIL::Command *cmd)
Perform given command.
Definition: be_adapter.cpp:332
virtual bool start()
Start adapter.
Definition: be_adapter.cpp:177
virtual bool stop()
Stop adapter.
Definition: be_adapter.cpp:206
virtual void bb_interface_data_refreshed(fawkes::Interface *interface) noexcept
BlackBoard data refreshed notification.
Definition: be_adapter.cpp:382
BehaviorEnginePlexilAdapter(PLEXIL::AdapterExecInterface &execInterface)
Constructor.
Definition: be_adapter.cpp:42
BlackBoard interface listener.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
virtual const char * path() const =0
Path of value.
Interface for configuration handling.
Definition: config.h:68
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
Uuid serial() const
Get instance serial of interface.
Definition: interface.cpp:695
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Interface for logging.
Definition: logger.h:42
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_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
unsigned int id() const
Get message ID.
Definition: message.cpp:181
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:95
void ref()
Increment reference count.
Definition: refcount.cpp:67
AcquireControlMessage Fawkes BlackBoard Interface Message.
ExecSkillMessage Fawkes BlackBoard Interface Message.
StopExecMessage Fawkes BlackBoard Interface Message.
SkillerInterface Fawkes BlackBoard Interface.
char * exclusive_controller() const
Get exclusive_controller value.
std::string get_string() const
Get the string representation of the Uuid.
Definition: uuid.cpp:107
Fawkes library namespace.