22 #include "be_adapter.h"
24 #include <utils/misc/map_skill.h>
26 #include <AdapterConfiguration.hh>
27 #include <AdapterExecInterface.hh>
28 #include <AdapterFactory.hh>
43 PLEXIL::AdapterExecInterface &execInterface)
54 PLEXIL::AdapterExecInterface &execInterface,
55 pugi::xml_node
const xml)
71 logger_ =
reinterpret_cast<fawkes::Logger *
>(m_execInterface.getProperty(
"::Fawkes::Logger"));
73 reinterpret_cast<fawkes::BlackBoard *
>(m_execInterface.getProperty(
"::Fawkes::BlackBoard"));
77 std::string cfg_prefix;
79 std::string cfg_spec = config_->
get_string(
"/plexil/spec");
80 cfg_prefix =
"/plexil/" + cfg_spec +
"/";
87 current_cmd_ =
nullptr;
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();
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);
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);
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));
114 cfg_skills_[id].args.resize(args_id + 1);
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);
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;
133 "Invalid argument type '%s' for '%s'",
135 cfg_skills_[
id].name.c_str());
137 }
else if (args_what ==
"name") {
138 cfg_skills_[id].args[args_id].name = cfg_item->get_as_string();
144 PLEXIL::g_configuration->registerCommandInterface(
"skill_call",
this);
146 std::map<std::string, std::string> mapping;
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 +
" (";
153 for (
const auto &arg : skill.args) {
159 line += PLEXIL::valueTypeName(arg.type) +
" " + arg.name;
161 line +=
") -> " + skill.template_str;
162 logger_->
log_debug(
"PlexilBE",
"%s", line.c_str());
164 mapping[skill.name] = skill.template_str;
165 PLEXIL::g_configuration->registerCommandInterface(skill.name,
this);
168 action_skill_mapping_ = std::make_shared<fawkes::ActionSkillMapping>(mapping);
191 logger_->
log_error(
"PlexilBE",
"No writer for skiller interface");
227 blackboard_->
close(skiller_if_);
232 BehaviorEnginePlexilAdapter::format_skillstring(
const std::vector<PLEXIL::Value> &values)
235 if (values.size() % 2 == 0) {
237 "Malformed skill call, must be 'skillname argname0 argvalue1...'");
238 }
else if (values.size() > 0) {
239 rv = values[0].valueToString() +
"{";
241 for (
size_t i = 1; i < values.size() - 1; i += 2) {
248 rv += values[i].valueToString() +
"=";
249 if (values[i + 1].valueType() == PLEXIL::STRING_TYPE) {
250 rv +=
"\"" + values[i + 1].valueToString() +
"\"";
252 rv += values[i + 1].valueToString();
262 BehaviorEnginePlexilAdapter::map_skillstring(
const std::string & name,
263 const skill_config & skill_config,
264 const std::vector<PLEXIL::Value> &values)
266 if (skill_config.args.size() != values.size()) {
268 "Arguments for '%s' do not match spec (got %zu, expected %zu)",
270 skill_config.args.size(),
274 for (
size_t i = 0; i < skill_config.args.size(); ++i) {
275 if (skill_config.args[i].type != values[i].valueType()) {
277 "Arguments type mismatch for '%s' of '%s' (got %s, expected %s)",
278 skill_config.args[i].name.c_str(),
280 PLEXIL::valueTypeName(values[i].valueType()).c_str(),
281 PLEXIL::valueTypeName(skill_config.args[i].type).c_str());
285 if (!action_skill_mapping_->has_mapping(name)) {
286 logger_->
log_warn(
"PlexilBE",
"No mapping for action '%s' known", name.c_str());
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();
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());
305 logger_->
log_info(
"PlexilBE",
"%s", m.second.c_str());
312 BehaviorEnginePlexilAdapter::call_skill(
const std::string &skill_string, PLEXIL::Command *cmd)
314 logger_->
log_info(
"PlexilBE",
"Executing skill '%s'", skill_string.c_str());
321 skill_msgid_ = msg->
id();
322 skill_string_ = skill_string;
334 std::lock_guard<std::mutex> lock(exec_mutex_);
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);
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;
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);
351 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
354 logger_->
log_warn(
"PlexilBE",
"Called for unknown skill '%s'", name.c_str());
355 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
359 m_execInterface.notifyOfExternalEvent();
368 logger_->
log_warn(
"PlexilBE",
"Aborting %s", cmd->getName().c_str());
374 current_cmd_ =
nullptr;
375 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
376 m_execInterface.handleCommandAbortAck(cmd,
false);
377 m_execInterface.notifyOfExternalEvent();
384 std::lock_guard<std::mutex> lock(exec_mutex_);
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;
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;
402 if (current_cmd_->getCommandHandle() == PLEXIL::COMMAND_SENT_TO_SYSTEM) {
403 m_execInterface.handleCommandAck(current_cmd_, PLEXIL::COMMAND_RCVD_BY_SYSTEM);
412 initBehaviorEngineAdapter()