23 #include "blackboard_adapter.h"
27 #include <utils/misc/string_conversions.h>
28 #include <utils/time/time.h>
30 #include <AdapterConfiguration.hh>
31 #include <AdapterExecInterface.hh>
32 #include <AdapterFactory.hh>
33 #include <CachedValue.hh>
37 #include <StateCacheEntry.hh>
60 pugi::xml_node
const xml)
76 logger_ = reinterpret_cast<fawkes::Logger *>(m_execInterface.getProperty(
"::Fawkes::Logger"));
78 reinterpret_cast<fawkes::BlackBoard *>(m_execInterface.getProperty(
"::Fawkes::BlackBoard"));
80 namespace p = std::placeholders;
81 commands_ = {{
"BB_open_for_reading",
82 std::bind(&BlackboardPlexilAdapter::bb_open_for_reading,
this, p::_1)},
83 {
"BB_close", std::bind(&BlackboardPlexilAdapter::bb_close,
this, p::_1)},
84 {
"BB_read", std::bind(&BlackboardPlexilAdapter::bb_read,
this, p::_1)},
85 {
"BB_read_all", std::bind(&BlackboardPlexilAdapter::bb_read_all,
this, p::_1)},
86 {
"BB_print", std::bind(&BlackboardPlexilAdapter::bb_print,
this, p::_1)}};
88 std::vector<std::string> lookups = {
"BB_changed",
98 for (
const auto &c : commands_) {
99 PLEXIL::g_configuration->registerCommandInterface(c.first,
this);
102 for (
const auto &l : lookups) {
103 PLEXIL::g_configuration->registerLookupInterface(l,
this);
106 blackboard_->register_listener(
this, BlackBoard::BBIL_FLAG_DATA);
146 for (
const auto &if_entry : ifs_read_) {
147 debugMsg(
"BlackboardAdapter:close",
148 "Closing " << if_entry.second->type() <<
"::" << if_entry.second->id());
149 blackboard_->
close(if_entry.second);
162 std::vector<PLEXIL::Value>
const ¶ms = state.parameters();
163 if (state.name() ==
"BB_changed") {
164 if (!verify_args(params,
"BlackboardAdapter:BB_changed", {{
"uid", PLEXIL::STRING_TYPE}})) {
165 cache_entry.setUnknown();
169 params[0].getValue(uid);
171 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
172 if (ifs_read_.find(uid) == ifs_read_.end()) {
174 "BB_changed: unknown interface %s, forgot to open?",
176 cache_entry.setUnknown();
179 cache_entry.update(ifs_read_[uid]->changed());
180 }
else if (state.name() ==
"BB_int" || state.name() ==
"BB_int_at" || state.name() ==
"BB_real"
181 || state.name() ==
"BB_real_at" || state.name() ==
"BB_bool"
182 || state.name() ==
"BB_bool_at" || state.name() ==
"BB_string"
183 || state.name() ==
"BB_field_length") {
184 bool is_indexed_version =
false;
185 if (state.name() ==
"BB_int" || state.name() ==
"BB_real" || state.name() ==
"BB_bool"
186 || state.name() ==
"BB_string" || state.name() ==
"BB_field_length") {
187 if (!verify_args(params,
188 "BlackboardAdapter:" + state.name(),
189 {{
"uid", PLEXIL::STRING_TYPE}, {
"field", PLEXIL::STRING_TYPE}})) {
190 cache_entry.setUnknown();
194 is_indexed_version =
true;
195 if (!verify_args(params,
196 "BlackboardAdapter:" + state.name(),
197 {{
"uid", PLEXIL::STRING_TYPE},
198 {
"field", PLEXIL::STRING_TYPE},
199 {
"index", PLEXIL::INTEGER_TYPE}})) {
200 cache_entry.setUnknown();
207 params[0].getValue(uid);
208 params[1].getValue(field);
210 if (is_indexed_version) {
211 params[2].getValue(index);
214 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
215 if (ifs_read_.find(uid) == ifs_read_.end()) {
217 "%s: unknown interface %s, forgot to open?",
218 state.name().c_str(),
220 cache_entry.setUnknown();
224 if (state.name().compare(0, 6,
"BB_int") == 0) {
225 for (
auto f = ifs_read_[uid]->fields(); f != ifs_read_[uid]->fields_end(); ++f) {
226 if (field == f.get_name()) {
229 PLEXIL::Integer value;
230 switch (f.get_type()) {
231 case IFT_INT8: value = f.get_int8(index);
break;
232 case IFT_UINT8: value = f.get_uint8(index);
break;
233 case IFT_INT16: value = f.get_int16(index);
break;
234 case IFT_UINT16: value = f.get_uint16(index);
break;
235 case IFT_INT32: value = f.get_int32(index);
break;
236 case IFT_UINT32: value = f.get_uint32(index);
break;
237 case IFT_INT64: value = f.get_int64(index);
break;
238 case IFT_UINT64: value = f.get_uint64(index);
break;
239 default: valid =
false;
break;
242 cache_entry.update(value);
245 "BB_int: field '%s' of %s of type %s",
249 cache_entry.setUnknown();
254 }
else if (state.name().compare(0, 7,
"BB_real") == 0) {
255 for (
auto f = ifs_read_[uid]->fields(); f != ifs_read_[uid]->fields_end(); ++f) {
256 if (field == f.get_name()) {
260 switch (f.get_type()) {
261 case IFT_FLOAT: value = f.get_float(index);
break;
262 case IFT_DOUBLE: value = f.get_double(index);
break;
263 case IFT_INT8: value = f.get_int8(index);
break;
264 case IFT_UINT8: value = f.get_uint8(index);
break;
265 case IFT_INT16: value = f.get_int16(index);
break;
266 case IFT_UINT16: value = f.get_uint16(index);
break;
267 case IFT_INT32: value = f.get_int32(index);
break;
268 case IFT_UINT32: value = f.get_uint32(index);
break;
269 case IFT_INT64: value = f.get_int64(index);
break;
270 case IFT_UINT64: value = f.get_uint64(index);
break;
271 default: valid =
false;
break;
274 cache_entry.update(value);
277 "BB_int: field %s of %s of type %s",
281 cache_entry.setUnknown();
286 }
else if (state.name().compare(0, 7,
"BB_bool") == 0) {
287 for (
auto f = ifs_read_[uid]->fields(); f != ifs_read_[uid]->fields_end(); ++f) {
288 if (field == f.get_name()) {
290 PLEXIL::Boolean value;
292 value = f.get_bool(index);
293 cache_entry.update(value);
296 "BB_int: field %s of %s of type %s",
300 cache_entry.setUnknown();
305 }
else if (state.name() ==
"BB_string") {
306 for (
auto f = ifs_read_[uid]->fields(); f != ifs_read_[uid]->fields_end(); ++f) {
307 if (field == f.get_name()) {
309 PLEXIL::String value;
310 value = f.get_value_string();
311 cache_entry.update(value);
315 }
else if (state.name() ==
"BB_field_length") {
316 for (
auto f = ifs_read_[uid]->fields(); f != ifs_read_[uid]->fields_end(); ++f) {
317 if (field == f.get_name()) {
319 PLEXIL::Integer value;
320 value = f.get_length();
321 cache_entry.update(value);
328 "%s: unknown field '%s' for interface %s",
329 state.name().c_str(),
332 cache_entry.setUnknown();
337 logger_->
log_warn(
"PlexilBB",
"unknown lookup '%s'", state.name().c_str());
338 cache_entry.setUnknown();
349 std::vector<PLEXIL::Value>
const ¶ms = state.parameters();
350 if (params.size() == 0 || params[0].valueType() != PLEXIL::STRING_TYPE) {
351 logger_->
log_error(
"PlexilBB",
"Invalid asynchronous lookup for %s", state.name().c_str());
355 params[0].getValue(uid);
357 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
358 if (ifs_read_.find(uid) == ifs_read_.end()) {
360 "Invalid asynchronous lookup %s for unknown interface %s",
361 state.name().c_str(),
364 if (subscribed_states_.count(uid) == 0) {
370 debugMsg(
"BlackboardAdapter:subscribe",
"Subscribe for " << state.toString());
371 subscribed_states_.insert({uid, state});
381 std::vector<PLEXIL::Value>
const ¶ms = state.parameters();
382 if (params.size() == 0 || params[0].valueType() != PLEXIL::STRING_TYPE) {
383 logger_->
log_error(
"PlexilBB",
"Invalid asynchronous lookup for %s", state.name().c_str());
387 params[0].getValue(uid);
389 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
390 if (ifs_read_.find(uid) == ifs_read_.end()) {
392 "Invalid lookup %s unsubscribe for "
393 "unknown interface %s",
394 state.name().c_str(),
398 debugMsg(
"BlackboardAdapter:unsubscribe",
"Unsubscribe for " << state.toString());
399 auto range = subscribed_states_.equal_range(uid);
400 for (
auto i = range.first; i != range.second; ++i) {
401 if (i->second == state) {
402 subscribed_states_.erase(i);
406 if (subscribed_states_.count(uid) == 0) {
414 BlackboardPlexilAdapter::bb_interface_data_changed(
fawkes::Interface *interface)
throw()
418 auto range = subscribed_states_.equal_range(interface->uid());
419 for (
auto i = range.first; i != range.second; ++i) {
420 if (i->second.name() ==
"BB_changed") {
425 m_execInterface.handleValueChange(i->second, PLEXIL::Value(
true));
427 PLEXIL::StateCacheEntry entry;
428 lookupNow(i->second, entry);
429 m_execInterface.handleValueChange(i->second, entry.cachedValue()->toValue());
432 m_execInterface.notifyOfExternalEvent();
441 std::string
const &name = cmd->getName();
443 auto c = commands_.find(name);
444 if (c != commands_.end()) {
447 warn(
"NavGraphAdapter:executeCommand: called for unknown"
450 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
451 m_execInterface.notifyOfExternalEvent();
456 BlackboardPlexilAdapter::bb_open_for_reading(PLEXIL::Command *cmd)
458 std::vector<PLEXIL::Value>
const &args = cmd->getArgValues();
459 if (!verify_args(args,
460 "BlackboardAdapter:bb_open_for_reading",
461 {{
"type", PLEXIL::STRING_TYPE}, {
"id", PLEXIL::STRING_TYPE}})) {
462 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
463 m_execInterface.notifyOfExternalEvent();
469 args[0].getValue(if_type);
470 args[1].getValue(if_id);
472 std::string uid{if_type +
"::" + if_id};
475 debugMsg(
"BlackboardAdapter:open",
"Open for reading " << uid);
477 if (ifs_read_.find(uid) == ifs_read_.end()) {
479 ifs_read_[uid] = blackboard_->
open_for_reading(if_type.c_str(), if_id.c_str());
480 PLEXIL::g_configuration->registerLookupInterface(uid +
".changed",
this);
483 "Failed to open interface %s:%s: %s",
487 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
488 m_execInterface.notifyOfExternalEvent();
493 m_execInterface.handleCommandReturn(cmd, PLEXIL::Value(
true));
494 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
495 m_execInterface.notifyOfExternalEvent();
499 BlackboardPlexilAdapter::bb_close(PLEXIL::Command *cmd)
501 std::vector<PLEXIL::Value>
const &args = cmd->getArgValues();
502 if (!verify_args(args,
"BlackboardAdapter:bb_close", {{
"uid", PLEXIL::STRING_TYPE}})) {
503 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
504 m_execInterface.notifyOfExternalEvent();
509 args[0].getValue(uid);
512 debugMsg(
"BlackboardAdapter:close",
"Close " << uid);
514 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
515 if (ifs_read_.find(uid) == ifs_read_.end()) {
516 logger_->
log_warn(
"PlexilBB",
"Interface '%s' has not been opened", uid.c_str());
517 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
518 m_execInterface.notifyOfExternalEvent();
522 blackboard_->
close(ifs_read_[uid]);
523 ifs_read_.erase(uid);
525 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
526 m_execInterface.notifyOfExternalEvent();
530 BlackboardPlexilAdapter::bb_read(PLEXIL::Command *cmd)
532 std::vector<PLEXIL::Value>
const &args = cmd->getArgValues();
533 if (!verify_args(args,
"BlackboardAdapter:bb_read", {{
"uid", PLEXIL::STRING_TYPE}})) {
534 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
535 m_execInterface.notifyOfExternalEvent();
540 args[0].getValue(uid);
543 debugMsg(
"BlackboardAdapter:read",
"Reading " << uid.c_str());
545 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
546 if (ifs_read_.find(uid) == ifs_read_.end()) {
547 logger_->
log_warn(
"PlexilBB",
"Interface '%s' has not been opened for reading", uid.c_str());
548 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
549 m_execInterface.notifyOfExternalEvent();
553 ifs_read_[uid]->read();
555 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
556 m_execInterface.notifyOfExternalEvent();
560 BlackboardPlexilAdapter::bb_read_all(PLEXIL::Command *cmd)
563 debugMsg(
"BlackboardAdapter:read",
"Reading all interfaces");
565 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
566 for (
auto &i : ifs_read_) {
570 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
571 m_execInterface.notifyOfExternalEvent();
575 BlackboardPlexilAdapter::bb_print(PLEXIL::Command *cmd)
577 std::vector<PLEXIL::Value>
const &args = cmd->getArgValues();
578 if (!verify_args(args,
"BlackboardAdapter:bb_print", {{
"uid", PLEXIL::STRING_TYPE}})) {
579 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
580 m_execInterface.notifyOfExternalEvent();
585 args[0].getValue(uid);
587 std::unique_lock<std::mutex> lock(ifs_read_mutex_);
588 if (ifs_read_.find(uid) == ifs_read_.end()) {
589 logger_->
log_warn(
"PlexilBB",
"Interface '%s' has not been opened for reading", uid.c_str());
590 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
591 m_execInterface.notifyOfExternalEvent();
601 std::string fact = std::string(
"(bb-data \"type\" \"") + i->
type() +
"\"" +
" \"id\" \""
602 + i->
id() +
"\"" +
" \"time\" " + StringConversions::to_string(t->
get_sec())
603 +
" " + StringConversions::to_string(t->
get_usec()) +
"" +
" (. ";
606 for (f = i->
fields(); f != f_end; ++f) {
610 std::string::size_type pos = 0;
611 while ((pos = value.find(
"\"", pos)) != std::string::npos) {
612 value.replace(pos, 1,
"\\\"");
615 value = std::string(
"\"") + value +
"\"";
616 }
else if (f.get_type() ==
IFT_ENUM) {
617 value = std::string(
"\"") + f.get_value_string(
" ") +
"\"";
619 value = f.get_value_string(
" ");
620 std::string::size_type pos;
621 while ((pos = value.find(
",")) != std::string::npos) {
622 value = value.erase(pos, 1);
625 if (f.get_length() > 1) {
626 fact += std::string(
" \"") + f.get_name() +
"\" [ " + value +
" ]";
628 fact += std::string(
" \"") + f.get_name() +
"\" " + value;
632 logger_->
log_info(
"PlexilBB",
"%s", fact.c_str());
635 "Failed to print interface '%s': %s",
638 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_FAILED);
639 m_execInterface.notifyOfExternalEvent();
643 m_execInterface.handleCommandAck(cmd, PLEXIL::COMMAND_SUCCESS);
644 m_execInterface.notifyOfExternalEvent();
649 initFawkesBlackboardAdapter()