22 #include "plexil_thread.h"
24 #include "be_adapter.h"
25 #include "clock_adapter.h"
26 #include "log_adapter.h"
27 #include "log_stream.h"
28 #include "protobuf_adapter.h"
29 #include "thread_adapter.h"
31 # include "navgraph_access_thread.h"
32 # include "navgraph_adapter.h"
36 #include <core/threading/mutex_locker.h>
37 #include <utils/sub_process/proc.h>
38 #include <utils/system/dynamic_module/module.h>
40 #include <AdapterConfiguration.hh>
42 #include <ExecApplication.hh>
43 #include <InterfaceManager.hh>
44 #include <InterfaceSchema.hh>
45 #include <boost/filesystem.hpp>
46 #include <boost/interprocess/sync/file_lock.hpp>
50 #include <pugixml.hpp>
53 namespace fs = boost::filesystem;
65 :
Thread(
"PlexilExecutiveThread",
Thread::OPMODE_CONTINUOUS)
80 std::string cfg_prefix =
"/plexil/" + cfg_spec_ +
"/";
84 std::map<std::string, plexil_interface_config> cfg_adapters =
85 read_plexil_interface_configs(cfg_prefix +
"adapters/");
87 std::map<std::string, plexil_interface_config> cfg_listeners =
88 read_plexil_interface_configs(cfg_prefix +
"listeners/");
90 std::vector<std::string> cfg_lib_path =
93 std::string cfg_basedir =
96 for (
auto &a_item : cfg_adapters) {
97 auto &a = a_item.second;
98 if (a.type ==
"Utility") {
99 logger->
log_warn(
name(),
"Utility adapter configured, consider using FawkesLogging instead");
100 }
else if (a.type ==
"OSNativeTime") {
102 "OSNativeTime adapter configured, consider using FawkesTime instead");
103 }
else if (a.type ==
"FawkesRemoteAdapter") {
105 throw Exception(
"Plexil: cannot load FawkesRemoteAdapter when running internally");
108 std::string filename =
110 if (fs::exists(filename)) {
111 a.attr[
"LibPath"] = filename;
115 plexil_.reset(
new PLEXIL::ExecApplication);
117 PLEXIL::g_manager->setProperty(
"::Fawkes::Config",
config);
118 PLEXIL::g_manager->setProperty(
"::Fawkes::Clock",
clock);
119 PLEXIL::g_manager->setProperty(
"::Fawkes::Logger",
logger);
120 PLEXIL::g_manager->setProperty(
"::Fawkes::BlackBoard",
blackboard);
122 for (
const auto &p : cfg_lib_path) {
123 plexil_->addLibraryPath(p);
126 pugi::xml_document xml_config;
127 pugi::xml_node xml_interfaces =
128 xml_config.append_child(PLEXIL::InterfaceSchema::INTERFACES_TAG());
130 add_plexil_interface_configs(xml_interfaces,
132 PLEXIL::InterfaceSchema::ADAPTER_TAG(),
133 PLEXIL::InterfaceSchema::ADAPTER_TYPE_ATTR());
134 add_plexil_interface_configs(xml_interfaces,
136 PLEXIL::InterfaceSchema::LISTENER_TAG(),
137 PLEXIL::InterfaceSchema::LISTENER_TYPE_ATTR());
139 auto navgraph_adapter_config =
140 std::find_if(cfg_adapters.begin(), cfg_adapters.end(), [](
const auto &entry) {
141 return entry.second.type ==
"NavGraphAdapter";
143 if (navgraph_adapter_config != cfg_adapters.end()) {
146 thread_collector->add(navgraph_access_thread_);
147 navgraph_ = navgraph_access_thread_->get_navgraph();
148 PLEXIL::g_manager->setProperty(
"::Fawkes::NavGraph", &navgraph_);
150 throw Exception(
"NavGraph adapter configured, "
151 "but navgraph library not available at compile time");
156 struct xml_string_writer : pugi::xml_writer
160 write(
const void *data,
size_t size)
162 result.append(
static_cast<const char *
>(data), size);
166 xml_string_writer writer;
167 xml_config.save(writer);
172 std::vector<std::string> debug_markers =
175 std::stringstream dbg_config;
176 for (
const auto &m : debug_markers) {
177 dbg_config << m << std::endl;
179 PLEXIL::readDebugConfigStream(dbg_config);
183 log_stream_.reset(
new std::ostream(&*log_buffer_));
184 PLEXIL::setDebugOutputStream(*log_stream_);
186 if (!plexil_->initialize(xml_interfaces)) {
187 throw Exception(
"Failed to initialize Plexil application");
195 cfg_plan_ple_ = {ple};
198 if (cfg_plan_ple_.empty()) {
202 cfg_plan_auto_compile_ =
204 cfg_plan_force_compile_ =
207 if (!cfg_plan_plx_.empty()) {
208 cfg_plan_plx_ = cfg_basedir +
"/" + cfg_plan_plx_;
209 replace_tokens(cfg_plan_plx_);
212 std::set<std::string> base_paths;
214 for (
auto &p : cfg_plan_ple_) {
215 p = cfg_basedir +
"/" + p;
218 fs::path ple_path{p};
219 fs::path plx_path{fs::path{ple_path}.replace_extension(
".plx")};
222 boost::interprocess::file_lock flock(ple_path.string().c_str());
224 base_paths.insert(plx_path.parent_path().string());
226 if (cfg_plan_auto_compile_) {
227 if (cfg_plan_force_compile_ || !fs::exists(plx_path)
228 || fs::last_write_time(plx_path) < fs::last_write_time(ple_path)) {
230 plexil_compile(ple_path.string());
233 if (!fs::exists(plx_path)) {
234 throw Exception(
"PLX %s does not exist and auto-compile disabled");
235 }
else if (fs::last_write_time(plx_path) < fs::last_write_time(ple_path)) {
237 "PLX %s older than PLE, auto-compile disabled",
238 plx_path.string().c_str());
243 if (!fs::exists(cfg_plan_plx_)) {
244 throw Exception(
"PLX %s does not exist", cfg_plan_plx_.c_str());
247 for (
const auto &p : base_paths) {
248 plexil_->addLibraryPath(p);
251 plan_plx_.reset(
new pugi::xml_document);
252 pugi::xml_parse_result parse_result = plan_plx_->load_file(cfg_plan_plx_.c_str());
253 if (parse_result.status != pugi::status_ok) {
254 throw Exception(
"Failed to parse plan '%s': %s",
255 cfg_plan_plx_.c_str(),
256 parse_result.description());
263 if (!plexil_->startInterfaces()) {
264 throw Exception(
"Failed to start Plexil interfaces");
266 if (!plexil_->run()) {
267 throw Exception(
"Failed to start Plexil");
270 if (!plexil_->addPlan(&*plan_plx_)) {
273 plexil_->notifyExec();
280 if (!plexil_->stop()) {
283 plexil_->notifyExec();
290 if (!plexil_->shutdown()) {
293 PLEXIL::g_configuration->clearAdapterRegistry();
294 plexil_->waitForShutdown();
309 thread_collector->remove(navgraph_access_thread_);
310 delete navgraph_access_thread_;
320 static PLEXIL::ExecApplication::ApplicationState state = PLEXIL::ExecApplication::APP_SHUTDOWN;
321 PLEXIL::ExecApplication::ApplicationState new_state = plexil_->getApplicationState();
322 if (new_state != state) {
323 logger->
log_info(
name(),
"State changed to %s", plexil_->getApplicationStateName(new_state));
327 using namespace std::chrono_literals;
328 std::this_thread::sleep_for(500ms);
332 std::map<std::string, PlexilExecutiveThread::plexil_interface_config>
333 PlexilExecutiveThread::read_plexil_interface_configs(
const std::string &config_prefix)
335 std::map<std::string, plexil_interface_config> cfg_adapters;
337 std::unique_ptr<Configuration::ValueIterator> cfg_item{
config->
search(config_prefix)};
338 while (cfg_item->next()) {
339 std::string path = cfg_item->
path();
341 std::string::size_type start_pos = config_prefix.size();
342 std::string::size_type slash_pos = path.find(
"/", start_pos + 1);
343 if (slash_pos != std::string::npos) {
344 std::string
id = path.substr(start_pos, slash_pos - start_pos);
346 start_pos = slash_pos + 1;
347 slash_pos = path.find(
"/", start_pos);
348 std::string what = path.substr(start_pos, slash_pos - start_pos);
350 if (what ==
"type") {
351 cfg_adapters[id].type = cfg_item->get_string();
352 }
else if (what ==
"attr") {
353 start_pos = slash_pos + 1;
354 slash_pos = path.find(
"/", start_pos);
355 std::string key = path.substr(start_pos, slash_pos - start_pos);
356 cfg_adapters[id].attr[key] = cfg_item->get_as_string();
357 }
else if (what ==
"args") {
358 start_pos = slash_pos + 1;
359 slash_pos = path.find(
"/", start_pos);
360 std::string key = path.substr(start_pos, slash_pos - start_pos);
361 cfg_adapters[id].args[key] = cfg_item->get_as_string();
362 }
else if (what ==
"verbatim-args") {
363 start_pos = slash_pos + 1;
364 slash_pos = path.find(
"/", start_pos);
365 std::string verb_id = path.substr(start_pos, slash_pos - start_pos);
367 start_pos = slash_pos + 1;
368 slash_pos = path.find(
"/", start_pos);
369 std::string verb_what = path.substr(start_pos, slash_pos - start_pos);
371 if (verb_what ==
"tag") {
372 cfg_adapters[id].verbatim_args[verb_id].tag = cfg_item->get_as_string();
373 }
else if (verb_what ==
"text") {
374 cfg_adapters[id].verbatim_args[verb_id].has_text =
true;
375 cfg_adapters[id].verbatim_args[verb_id].text = cfg_item->get_as_string();
376 }
else if (verb_what ==
"attr") {
377 start_pos = slash_pos + 1;
378 slash_pos = path.find(
"/", start_pos);
379 std::string verb_key = path.substr(start_pos, slash_pos - start_pos);
380 cfg_adapters[id].verbatim_args[verb_id].attr[verb_key] = cfg_item->get_as_string();
382 }
else if (what ==
"verbatim-xml") {
384 pugi::xml_parse_result parse_result =
385 cfg_adapters[id].verbatim.load_string(cfg_item->get_string().c_str());
386 if (parse_result.status != pugi::status_ok) {
387 throw Exception(
"Failed to parse verbatim-xml for '%s': %s",
388 cfg_adapters[
id].type.c_str(),
389 parse_result.description());
399 PlexilExecutiveThread::add_plexil_interface_configs(
400 pugi::xml_node & parent,
401 const std::map<std::string, PlexilExecutiveThread::plexil_interface_config> &configs,
402 const char * tag_name,
403 const char * type_attr_name)
405 for (
const auto &a_item : configs) {
406 const auto & a = a_item.second;
407 pugi::xml_node xml_adapter = parent.append_child(tag_name);
408 xml_adapter.append_attribute(type_attr_name).set_value(a.type.c_str());
409 for (
const auto &attr : a.attr) {
410 xml_adapter.append_attribute(attr.first.c_str()).set_value(attr.second.c_str());
412 for (
const auto &arg : a.args) {
413 pugi::xml_node xml_adapter_arg = xml_adapter.append_child(
"Parameter");
414 xml_adapter_arg.append_attribute(
"key").set_value(arg.first.c_str());
415 xml_adapter_arg.text().set(arg.second.c_str());
417 for (
const auto &arg : a.verbatim_args) {
418 const auto & varg = arg.second;
419 pugi::xml_node xml_adapter_arg = xml_adapter.append_child(varg.tag.c_str());
420 for (
const auto &attr : varg.attr) {
421 xml_adapter_arg.append_attribute(attr.first.c_str()).set_value(attr.second.c_str());
424 xml_adapter_arg.text().set(varg.text.c_str());
427 if (a.verbatim && a.verbatim.children().begin() != a.verbatim.children().end()) {
428 for (
const auto &child : a.verbatim.children()) {
429 xml_adapter.append_copy(child);
436 PlexilExecutiveThread::plexil_compile(
const std::string &ple_file)
438 std::vector<std::string> argv{
"plexilc", ple_file};
439 std::string command_line =
440 std::accumulate(std::next(argv.begin()),
443 [](std::string &s,
const std::string &a) { return s +
" " + a; });
447 using namespace std::chrono_literals;
448 auto compile_start = std::chrono::system_clock::now();
449 auto now = std::chrono::system_clock::now();
454 throw Exception(
"Plexil compilation failed, check log for messages.");
459 now = std::chrono::system_clock::now();
460 std::this_thread::sleep_for(500ms);
461 }
while (now < compile_start + 30s);
464 throw Exception(
"Plexil compilation timeout after 30s");